From c2ed45d6004baa86adc0cbeefcda94f03a1f36b3 Mon Sep 17 00:00:00 2001
From: Binit Shah <bshah@hello-robot.com>
Date: Thu, 16 Nov 2023 00:34:15 -0500
Subject: [PATCH] Revert xacros and point to gazebo copy

---
 stretch_gazebo/urdf/stretch_aruco.xacro       | 283 ++++++
 stretch_gazebo/urdf/stretch_d435i.xacro       |  23 +
 stretch_gazebo/urdf/stretch_dex_wrist.xacro   | 451 ++++++++++
 .../urdf/stretch_gazebo_dex_wrist.urdf.xacro  |  12 +-
 ...stretch_gazebo_standard_gripper.urdf.xacro |  12 +-
 stretch_gazebo/urdf/stretch_gripper.xacro     | 302 +++++++
 .../urdf/stretch_laser_range_finder.xacro     |  59 ++
 stretch_gazebo/urdf/stretch_main.xacro        | 841 ++++++++++++++++++
 stretch_gazebo/urdf/stretch_respeaker.xacro   |  62 ++
 9 files changed, 2033 insertions(+), 12 deletions(-)
 create mode 100644 stretch_gazebo/urdf/stretch_aruco.xacro
 create mode 100644 stretch_gazebo/urdf/stretch_d435i.xacro
 create mode 100644 stretch_gazebo/urdf/stretch_dex_wrist.xacro
 create mode 100644 stretch_gazebo/urdf/stretch_gripper.xacro
 create mode 100644 stretch_gazebo/urdf/stretch_laser_range_finder.xacro
 create mode 100644 stretch_gazebo/urdf/stretch_main.xacro
 create mode 100644 stretch_gazebo/urdf/stretch_respeaker.xacro

diff --git a/stretch_gazebo/urdf/stretch_aruco.xacro b/stretch_gazebo/urdf/stretch_aruco.xacro
new file mode 100644
index 0000000..48bb565
--- /dev/null
+++ b/stretch_gazebo/urdf/stretch_aruco.xacro
@@ -0,0 +1,283 @@
+<?xml version="1.0"?>
+<robot xmlns:xacro="http://www.ros.org/wiki/xacro" name="stretch_aruco">
+
+  <link
+    name="link_aruco_right_base">
+    <inertial>
+      <origin
+        xyz="1.3878E-17 0 -0.000125"
+        rpy="0 0 0" />
+      <mass
+        value="3.59999999990368E-06" />
+      <inertia
+        ixx="0"
+        ixy="0"
+        ixz="0"
+        iyy="0"
+        iyz="0"
+        izz="0" />
+    </inertial>
+    <visual>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <mesh
+          filename="package://stretch_description/meshes/link_aruco_right_base.STL" />
+      </geometry>
+      <material
+        name="">
+        <color
+          rgba="0.792156862745098 0.819607843137255 0.933333333333333 1" />
+      </material>
+    </visual>
+    <collision>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <mesh
+          filename="package://stretch_description/meshes/link_aruco_right_base.STL" />
+      </geometry>
+    </collision>
+  </link>
+  
+  <joint
+    name="joint_aruco_right_base"
+    type="fixed">
+    <origin
+      xyz="-0.0015028 -0.1304972 0.1597482"
+      rpy="0 0 -1.5707963267949" />
+    <parent
+      link="base_link" />
+    <child
+      link="link_aruco_right_base" />
+    <axis
+      xyz="0 0 0" />
+  </joint>
+
+  
+  <link
+    name="link_aruco_left_base">
+    <inertial>
+      <origin
+        xyz="2.7756E-17 0 -0.000125"
+        rpy="0 0 0" />
+      <mass
+        value="3.59999999990368E-06" />
+      <inertia
+        ixx="0"
+        ixy="0"
+        ixz="0"
+        iyy="0"
+        iyz="0"
+        izz="0" />
+    </inertial>
+    <visual>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <mesh
+          filename="package://stretch_description/meshes/link_aruco_left_base.STL" />
+      </geometry>
+      <material
+        name="">
+        <color
+          rgba="0.792156862745098 0.819607843137255 0.933333333333333 1" />
+      </material>
+    </visual>
+    <collision>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <mesh
+          filename="package://stretch_description/meshes/link_aruco_left_base.STL" />
+      </geometry>
+    </collision>
+  </link>
+  
+  <joint
+    name="joint_aruco_left_base"
+    type="fixed">
+    <origin
+      xyz="-0.00500000000000014 0.1304972 0.1597482"
+      rpy="0 0 -1.5707963267949" />
+    <parent
+      link="base_link" />
+    <child
+      link="link_aruco_left_base" />
+    <axis
+      xyz="0 0 0" />
+  </joint>
+  
+  <link
+    name="link_aruco_shoulder">
+    <inertial>
+      <origin
+        xyz="-2.77555756156289E-17 2.56739074444567E-16 -0.000125000000000042"
+        rpy="0 0 0" />
+      <mass
+        value="0" />
+      <inertia
+        ixx="0"
+        ixy="0"
+        ixz="0"
+        iyy="0"
+        iyz="0"
+        izz="0" />
+    </inertial>
+    <visual>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <mesh
+          filename="package://stretch_description/meshes/link_aruco_shoulder.STL" />
+      </geometry>
+      <material
+        name="">
+        <color
+          rgba="0.792156862745098 0.819607843137255 0.933333333333333 1" />
+      </material>
+    </visual>
+    <collision>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <mesh
+          filename="package://stretch_description/meshes/link_aruco_shoulder.STL" />
+      </geometry>
+    </collision>
+  </link>
+
+  <joint
+    name="joint_aruco_shoulder"
+    type="fixed">
+    <origin
+      xyz="-0.0133768876375287 0.0558540528812078 0.0861368272417975"
+      rpy="-1.53998860117704E-29 3.55962409571165E-15 0.0" />
+    <parent
+      link="link_lift" />
+    <child
+      link="link_aruco_shoulder" />
+    <axis
+      xyz="0 0 0" />
+  </joint>
+
+  <link
+    name="link_aruco_top_wrist">
+    <inertial>
+      <origin
+        xyz="-1.3531E-15 -3.4972E-15 -0.000125"
+        rpy="0 0 0" />
+      <mass
+        value="0" />
+      <inertia
+        ixx="0"
+        ixy="0"
+        ixz="0"
+        iyy="0"
+        iyz="0"
+        izz="0" />
+    </inertial>
+    <visual>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <mesh
+          filename="package://stretch_description/meshes/link_aruco_top_wrist.STL" />
+      </geometry>
+      <material
+        name="">
+        <color
+          rgba="0.792156862745098 0.819607843137255 0.933333333333333 1" />
+      </material>
+    </visual>
+    <collision>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <mesh
+          filename="package://stretch_description/meshes/link_aruco_top_wrist.STL" />
+      </geometry>
+    </collision>
+  </link>
+
+  <joint
+    name="joint_aruco_top_wrist"
+    type="fixed">
+    <origin
+      xyz="0.0472500000000019 0.0292850000000015 0"
+      rpy="1.5707963267949 -8.03728587323464E-15 3.14159265358979" />
+    <parent
+      link="link_arm_l0" />
+    <child
+      link="link_aruco_top_wrist" />
+    <axis
+      xyz="0 0 0" />
+  </joint>
+
+  <link
+    name="link_aruco_inner_wrist">
+    <inertial>
+      <origin
+        xyz="8.32667268468867E-17 1.77635683940025E-15 -0.000125000000000264"
+        rpy="0 0 0" />
+      <mass
+        value="0" />
+      <inertia
+        ixx="0"
+        ixy="0"
+        ixz="0"
+        iyy="0"
+        iyz="0"
+        izz="0" />
+    </inertial>
+    <visual>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <mesh
+          filename="package://stretch_description/meshes/link_aruco_inner_wrist.STL" />
+      </geometry>
+      <material
+        name="">
+        <color
+          rgba="0.792156862745098 0.819607843137255 0.933333333333333 1" />
+      </material>
+    </visual>
+    <collision>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <mesh
+          filename="package://stretch_description/meshes/link_aruco_inner_wrist.STL" />
+      </geometry>
+    </collision>
+  </link>
+
+  <joint
+    name="joint_aruco_inner_wrist"
+    type="fixed">
+    <origin
+      xyz="0.0472499999999947 -0.0119000000000034 -0.0272499999991938"
+      rpy="3.14159265358979 4.23377442363088E-14 3.14159265358979" />
+    <parent
+      link="link_arm_l0" />
+    <child
+      link="link_aruco_inner_wrist" />
+    <axis
+      xyz="0 0 0" />
+  </joint>
+
+</robot>
+
+  
+
diff --git a/stretch_gazebo/urdf/stretch_d435i.xacro b/stretch_gazebo/urdf/stretch_d435i.xacro
new file mode 100644
index 0000000..167383d
--- /dev/null
+++ b/stretch_gazebo/urdf/stretch_d435i.xacro
@@ -0,0 +1,23 @@
+<?xml version="1.0"?>
+<robot xmlns:xacro="http://www.ros.org/wiki/xacro" name="stretch_d435i">
+
+  <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"/>
+
+  <!-- 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)">
+    <origin
+      xyz="0.03 -0.0122 0.0182"
+      rpy="0.0 0.0 0.0" />
+  </xacro:sensor_d435i>
+
+  
+</robot>
+
+  
+
diff --git a/stretch_gazebo/urdf/stretch_dex_wrist.xacro b/stretch_gazebo/urdf/stretch_dex_wrist.xacro
new file mode 100644
index 0000000..379b9b4
--- /dev/null
+++ b/stretch_gazebo/urdf/stretch_dex_wrist.xacro
@@ -0,0 +1,451 @@
+<?xml version="1.0"?>
+<robot xmlns:xacro="http://www.ros.org/wiki/xacro" name="stretch_dex_wrist">
+
+  <xacro:property name="scale_finger_length" value="0.9" />
+
+  <link
+    name="link_wrist_yaw_bottom">
+    <inertial>
+      <origin
+        xyz="-0.012839101377342 -0.0382787718640742 -0.0228400332263617"
+        rpy="0 0 0" />
+      <mass
+        value="0.0988906816399982" />
+      <inertia
+        ixx="2.60067866573596E-05"
+        ixy="-6.73176267521354E-06"
+        ixz="-2.43476436723672E-06"
+        iyy="5.99482946819923E-06"
+        iyz="-3.39642410492401E-06"
+        izz="2.56907114334732E-05" />
+    </inertial>
+    <visual>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <mesh
+          filename="package://stretch_description/meshes/link_wrist_yaw_bottom.STL" />
+      </geometry>
+      <material
+        name="">
+        <color
+          rgba="0.792156862745098 0.819607843137255 0.933333333333333 1" />
+      </material>
+    </visual>
+    <collision>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <mesh
+          filename="package://stretch_description/meshes/link_wrist_yaw_bottom.STL" />
+      </geometry>
+    </collision>
+  </link>
+  <joint
+    name="joint_wrist_yaw_bottom"
+    type="fixed">
+    <origin
+      xyz="0 0 0"
+      rpy="-3.14159265358979 1.13367999021379E-14 1.57079632679489" />
+    <parent
+      link="link_wrist_yaw" />
+    <child
+      link="link_wrist_yaw_bottom" />
+    <axis
+      xyz="0 0 0" />
+  </joint>
+  <link
+    name="link_wrist_pitch">
+    <inertial>
+      <origin
+        xyz="-0.00310609611067142 -0.0150777141465843 0.0204734587925901"
+        rpy="0 0 0" />
+      <mass
+        value="0.0701267146295583" />
+      <inertia
+        ixx="2.55965614980905E-06"
+        ixy="-1.47551515167608E-06"
+        ixz="-6.31436085977252E-08"
+        iyy="3.43968637386282E-06"
+        iyz="-4.17813567208843E-07"
+        izz="4.53568668211393E-06" />
+    </inertial>
+    <visual>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <mesh
+          filename="package://stretch_description/meshes/link_wrist_pitch.STL" />
+      </geometry>
+      <material
+        name="">
+        <color
+          rgba="0.792156862745098 0.819607843137255 0.933333333333333 1" />
+      </material>
+    </visual>
+    <collision>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <mesh
+          filename="package://stretch_description/meshes/link_wrist_pitch.STL" />
+      </geometry>
+    </collision>
+  </link>
+  <joint
+    name="joint_wrist_pitch"
+    type="revolute">
+    <origin
+      xyz="0 -0.0195500000000002 -0.0247499999999984"
+      rpy="1.5707963267949 -8.12895570882604E-15 -3.14159265358979" />
+    <parent
+      link="link_wrist_yaw_bottom" />
+    <child
+      link="link_wrist_pitch" />
+    <axis
+      xyz="0 0 -1" />
+    <limit effort="100" lower="-1.57" upper="0.56" velocity="1.0"/>
+  </joint>
+  <link
+    name="link_wrist_roll">
+    <inertial>
+      <origin
+        xyz="9.63118473862323E-15 -6.38378239159465E-15 0.00768048802649798"
+        rpy="0 0 0" />
+      <mass
+        value="0.00585666394358811" />
+      <inertia
+        ixx="2.55965614980905E-06"
+        ixy="-1.47551515167608E-06"
+        ixz="-6.31436085977252E-08"
+        iyy="3.43968637386282E-06"
+        iyz="-4.17813567208843E-07"
+        izz="4.53568668211393E-06" />
+    </inertial>
+    <visual>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <mesh
+          filename="package://stretch_description/meshes/link_wrist_roll.STL" />
+      </geometry>
+      <material
+        name="">
+        <color
+          rgba="0.792156862745098 0.819607843137255 0.933333333333333 1" />
+      </material>
+    </visual>
+    <collision>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <mesh
+          filename="package://stretch_description/meshes/link_wrist_roll.STL" />
+      </geometry>
+    </collision>
+  </link>
+  <joint
+    name="joint_wrist_roll"
+    type="revolute">
+    <origin
+      xyz="-0.0188587444076125 -0.0239999999998942 0.01955"
+      rpy="3.14159265358979 1.5707963267949 0" />
+    <parent
+      link="link_wrist_pitch" />
+    <child
+      link="link_wrist_roll" />
+    <axis
+      xyz="0 0 1" />
+    <limit effort="100" lower="-3.14" upper="3.14" velocity="1.0"/>
+  </joint>
+  <link
+    name="link_straight_gripper">
+    <inertial>
+      <origin
+        xyz="0.00150764845432383 -0.00711581846201287 0.0399737901417758"
+        rpy="0 0 0" />
+      <mass
+        value="0.0496384234458284" />
+      <inertia
+        ixx="5.61461154156397E-06"
+        ixy="8.29518962984231E-07"
+        ixz="-2.41382921888194E-06"
+        iyy="1.11504692003467E-05"
+        iyz="9.76174898123369E-07"
+        izz="6.63803357903882E-06" />
+    </inertial>
+    <visual>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <mesh
+          filename="package://stretch_description/meshes/link_straight_gripper.STL" />
+      </geometry>
+      <material
+        name="">
+        <color
+          rgba="0.792156862745098 0.819607843137255 0.933333333333333 1" />
+      </material>
+    </visual>
+    <collision>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <mesh
+          filename="package://stretch_description/meshes/link_straight_gripper.STL" />
+      </geometry>
+    </collision>
+  </link>
+  <joint
+    name="joint_straight_gripper"
+    type="fixed">
+    <origin
+      xyz="0 0 0.0155"
+      rpy="3.54987407349455E-30 3.24021254484265E-20 -3.14159265358979" />
+    <parent
+      link="link_wrist_roll" />
+    <child
+      link="link_straight_gripper" />
+    <axis
+      xyz="0 0 0" />
+  </joint>
+  <link
+    name="link_gripper_finger_right">
+    <inertial>
+      <origin
+        xyz="-0.094981 -0.0080152 -2.2204E-16"
+        rpy="0 0 0" />
+      <mass
+        value="0.047621" />
+      <inertia
+        ixx="0.001"
+        ixy="0"
+        ixz="0"
+        iyy="0.001"
+        iyz="0"
+        izz="0.001" />
+    </inertial>
+    <visual>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <mesh
+          filename="package://stretch_description/meshes/link_gripper_finger_right.STL" />
+      </geometry>
+      <material
+        name="">
+        <color
+          rgba="0.79216 0.81961 0.93333 1" />
+      </material>
+    </visual>
+    <collision>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <mesh
+          filename="package://stretch_description/meshes/link_gripper_finger_right.STL" />
+      </geometry>
+    </collision>
+  </link>
+  <joint
+    name="joint_gripper_finger_right"
+    type="revolute">
+    <origin
+      xyz="-0.018599 0.003 0.033689"
+      rpy="1.5708 1.5708 0" />
+    <parent
+      link="link_straight_gripper" />
+    <child
+      link="link_gripper_finger_right" />
+    <axis
+      xyz="0 0 1" />
+    <limit effort="100" lower="-0.6" upper="0.6" velocity="1.0"/>
+  </joint>
+  <link
+    name="link_gripper_fingertip_right">
+    <inertial>
+      <origin
+        xyz="2.83785970833783E-08 6.75131661687089E-09 0.00812578923434215"
+        rpy="0 0 0" />
+      <mass
+        value="0.00382160881468841" />
+      <inertia
+        ixx="0.001"
+        ixy="0"
+        ixz="0"
+        iyy="0.001"
+        iyz="0"
+        izz="0.001" />
+    </inertial>
+    <visual>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <mesh
+          filename="package://stretch_description/meshes/link_gripper_fingertip_right.STL" />
+      </geometry>
+      <material
+        name="">
+        <color
+          rgba="0.250980392156863 0.250980392156863 0.250980392156863 1" />
+      </material>
+    </visual>
+    <collision>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <mesh
+          filename="package://stretch_description/meshes/link_gripper_fingertip_right.STL" />
+      </geometry>
+    </collision>
+  </link>
+  <joint
+    name="joint_gripper_fingertip_right"
+    type="fixed">
+    <origin
+      xyz="-0.190596948563868 -0.015 0"
+      rpy="-1.57079632679483 -3.43320051448326E-14 0.540456056432235" />
+    <parent
+      link="link_gripper_finger_right" />
+    <child
+      link="link_gripper_fingertip_right" />
+    <axis
+      xyz="0 0 0" />
+  </joint>
+  <link
+    name="link_gripper_finger_left">
+    <inertial>
+      <origin
+        xyz="0.0949811095686165 -0.00801522758203194 1.38777878078145E-15"
+        rpy="0 0 0" />
+      <mass
+        value="0.0476207785199479" />
+      <inertia
+        ixx="0.001"
+        ixy="0"
+        ixz="0"
+        iyy="0.001"
+        iyz="0"
+        izz="0.001" />
+    </inertial>
+    <visual>
+      <origin
+        xyz="0 0 0"
+        rpy="3.141592653589793 0 0" />
+      <geometry>
+        <mesh
+          filename="package://stretch_description/meshes/link_gripper_finger_left.STL" />
+      </geometry>
+      <material
+        name="">
+        <color
+          rgba="0.792156862745098 0.819607843137255 0.933333333333333 1" />
+      </material>
+    </visual>
+    <collision>
+      <origin
+        xyz="0 0 0"
+        rpy="1.5707963267948966 0 0" />
+      <geometry>
+        <mesh
+          filename="package://stretch_description/meshes/link_gripper_finger_left.STL" />
+      </geometry>
+    </collision>
+  </link>
+  <joint
+    name="joint_gripper_finger_left"
+    type="revolute">
+    <origin
+      xyz="0.018599 0.003 0.033689"
+      rpy="1.5708 -1.5708 0" />
+    <parent
+      link="link_straight_gripper" />
+    <child
+      link="link_gripper_finger_left" />
+    <axis
+      xyz="0 0 -1" />
+    <limit effort="100" lower="-0.6" upper="0.6" velocity="1.0"/>
+  </joint>
+  <link
+    name="link_gripper_fingertip_left">
+    <inertial>
+      <origin
+        xyz="-2.59496317767116E-08 -6.65612598371723E-09 0.00812579036862837"
+        rpy="0 0 0" />
+      <mass
+        value="0.00382160686584851" />
+      <inertia
+        ixx="0.001"
+        ixy="0"
+        ixz="0"
+        iyy="0.001"
+        iyz="0"
+        izz="0.001" />
+    </inertial>
+    <visual>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <mesh
+          filename="package://stretch_description/meshes/link_gripper_fingertip_left.STL" />
+      </geometry>
+      <material
+        name="">
+        <color
+          rgba="0.250980392156863 0.250980392156863 0.250980392156863 1" />
+      </material>
+    </visual>
+    <collision>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <mesh
+          filename="package://stretch_description/meshes/link_gripper_fingertip_left.STL" />
+      </geometry>
+    </collision>
+  </link>
+  <joint
+    name="joint_gripper_fingertip_left"
+    type="fixed">
+    <origin
+      xyz="0.190596948563868 -0.015 0"
+      rpy="1.57079632679496 4.51275387511463E-14 2.60113659715756" />
+    <parent
+      link="link_gripper_finger_left" />
+    <child
+      link="link_gripper_fingertip_left" />
+    <axis
+      xyz="0 0 0" />
+  </joint>
+
+  <link
+      name="link_grasp_center">
+  </link>
+
+  <joint
+    name="joint_grasp_center"
+    type="fixed">
+    <origin
+      xyz="0 0 0.23"
+      rpy="-1.5707963267949 -1.5707963267949 0" />
+    <parent
+      link="link_straight_gripper" />
+    <child
+      link="link_grasp_center" />
+  </joint>
+
+</robot>
\ No newline at end of file
diff --git a/stretch_gazebo/urdf/stretch_gazebo_dex_wrist.urdf.xacro b/stretch_gazebo/urdf/stretch_gazebo_dex_wrist.urdf.xacro
index 56a9512..83982c4 100644
--- a/stretch_gazebo/urdf/stretch_gazebo_dex_wrist.urdf.xacro
+++ b/stretch_gazebo/urdf/stretch_gazebo_dex_wrist.urdf.xacro
@@ -3,12 +3,12 @@
   <xacro:arg name="gpu_lidar" default="false" />
   <xacro:arg name="visualize_lidar" default="false" />
 
-  <xacro:include filename="$(find stretch_description)/urdf/stretch_main.xacro" />
-  <xacro:include filename="$(find stretch_description)/urdf/stretch_aruco.xacro" />
-  <xacro:include filename="$(find stretch_description)/urdf/stretch_d435i.xacro" />
-  <xacro:include filename="$(find stretch_description)/urdf/stretch_laser_range_finder.xacro" />
-  <xacro:include filename="$(find stretch_description)/urdf/stretch_respeaker.xacro" />
-  <xacro:include filename="$(find stretch_description)/urdf/stretch_dex_wrist.xacro" />
+  <xacro:include filename="$(find stretch_gazebo)/urdf/stretch_main.xacro" />
+  <xacro:include filename="$(find stretch_gazebo)/urdf/stretch_aruco.xacro" />
+  <xacro:include filename="$(find stretch_gazebo)/urdf/stretch_d435i.xacro" />
+  <xacro:include filename="$(find stretch_gazebo)/urdf/stretch_laser_range_finder.xacro" />
+  <xacro:include filename="$(find stretch_gazebo)/urdf/stretch_respeaker.xacro" />
+  <xacro:include filename="$(find stretch_gazebo)/urdf/stretch_dex_wrist.xacro" />
 
   <gazebo>
     <plugin name="gazebo_ros_control" filename="libgazebo_ros_control.so">
diff --git a/stretch_gazebo/urdf/stretch_gazebo_standard_gripper.urdf.xacro b/stretch_gazebo/urdf/stretch_gazebo_standard_gripper.urdf.xacro
index a1fc7aa..17fb961 100644
--- a/stretch_gazebo/urdf/stretch_gazebo_standard_gripper.urdf.xacro
+++ b/stretch_gazebo/urdf/stretch_gazebo_standard_gripper.urdf.xacro
@@ -3,12 +3,12 @@
   <xacro:arg name="gpu_lidar" default="false" />
   <xacro:arg name="visualize_lidar" default="false" />
 
-  <xacro:include filename="$(find stretch_description)/urdf/stretch_main.xacro" />
-  <xacro:include filename="$(find stretch_description)/urdf/stretch_aruco.xacro" />
-  <xacro:include filename="$(find stretch_description)/urdf/stretch_d435i.xacro" />
-  <xacro:include filename="$(find stretch_description)/urdf/stretch_laser_range_finder.xacro" />
-  <xacro:include filename="$(find stretch_description)/urdf/stretch_respeaker.xacro" />
-  <xacro:include filename="$(find stretch_description)/urdf/stretch_gripper.xacro" />
+  <xacro:include filename="$(find stretch_gazebo)/urdf/stretch_main.xacro" />
+  <xacro:include filename="$(find stretch_gazebo)/urdf/stretch_aruco.xacro" />
+  <xacro:include filename="$(find stretch_gazebo)/urdf/stretch_d435i.xacro" />
+  <xacro:include filename="$(find stretch_gazebo)/urdf/stretch_laser_range_finder.xacro" />
+  <xacro:include filename="$(find stretch_gazebo)/urdf/stretch_respeaker.xacro" />
+  <xacro:include filename="$(find stretch_gazebo)/urdf/stretch_gripper.xacro" />
 
   <gazebo>
     <plugin name="gazebo_ros_control" filename="libgazebo_ros_control.so">
diff --git a/stretch_gazebo/urdf/stretch_gripper.xacro b/stretch_gazebo/urdf/stretch_gripper.xacro
new file mode 100644
index 0000000..03acc68
--- /dev/null
+++ b/stretch_gazebo/urdf/stretch_gripper.xacro
@@ -0,0 +1,302 @@
+<?xml version="1.0"?>
+<robot xmlns:xacro="http://www.ros.org/wiki/xacro" name="stretch_gripper">
+
+  <xacro:property name="scale_finger_length" value="0.9" />
+
+  <link
+    name="link_gripper">
+    <inertial>
+      <origin
+        xyz="-0.0170229132730066 0.0131410320934285 -0.0371614759484659"
+        rpy="0 0 0" />
+      <mass
+        value="0.101902711393094" />
+      <inertia
+        ixx="0.001"
+        ixy="0"
+        ixz="0"
+        iyy="0.001"
+        iyz="0"
+        izz="0.001" />
+    </inertial>
+    <visual>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <mesh
+          filename="package://stretch_description/meshes/link_gripper.STL" />
+      </geometry>
+      <material
+        name="">
+        <color
+          rgba="0.792156862745098 0.819607843137255 0.933333333333333 1" />
+      </material>
+    </visual>
+    <collision>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <mesh
+          filename="package://stretch_description/meshes/link_gripper.STL" />
+      </geometry>
+    </collision>
+  </link>
+
+  <!-- rpy="3.1416 0 1.5708" -->
+  <!-- rpy="0.0 0 1.5708" -->
+  <joint
+    name="joint_gripper"
+    type="fixed">
+    <origin
+      xyz="0 0 0"
+      rpy="3.14159 0 -1.5708" />
+    <parent
+      link="link_wrist_yaw" />
+    <child
+      link="link_gripper" />
+    <axis
+      xyz="0 0 0" />
+  </joint>
+  
+  <link
+    name="link_gripper_finger_left">
+    <inertial>
+      <origin
+        xyz="0.0945047483510102 0.0124301080924361 -4.44089209850063E-16"
+        rpy="0 0 0" />
+      <mass
+        value="0.0476207785199474" />
+      <inertia
+        ixx="0.001"
+        ixy="0"
+        ixz="0"
+        iyy="0.001"
+        iyz="0"
+        izz="0.001" />
+    </inertial>
+    <visual>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <mesh
+          filename="package://stretch_description/meshes/link_gripper_finger_left.STL" scale="${scale_finger_length} 1.0 1.0"/>
+      </geometry>
+      <material
+        name="">
+        <color
+          rgba="0.792156862745098 0.819607843137255 0.933333333333333 1" />
+      </material>
+    </visual>
+    <collision>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <mesh
+          filename="package://stretch_description/meshes/link_gripper_finger_left.STL" scale="${scale_finger_length} 1.0 1.0"/>
+      </geometry>
+    </collision>
+  </link>
+  
+  <joint
+    name="joint_gripper_finger_left"
+    type="revolute">
+    <origin
+      xyz="-0.047231 -0.010151 -0.04679"
+      rpy="2.1762E-15 0.5236 3.1416" />
+    <parent
+      link="link_gripper" />
+    <child
+      link="link_gripper_finger_left" />
+    <axis
+      xyz="0 0 1" />
+    <limit effort="100" lower="-0.6" upper="0.6" velocity="1.0"/>
+  </joint>
+  
+  <link
+    name="link_gripper_fingertip_left">
+    <inertial>
+      <origin
+        xyz="-3.16381381648689E-08 -2.91408530639359E-09 0.00812579670381812"
+        rpy="0 0 0" />
+      <mass
+        value="0.00382159917455729" />
+      <inertia
+        ixx="0.001"
+        ixy="0"
+        ixz="0"
+        iyy="0.001"
+        iyz="0"
+        izz="0.001" />
+    </inertial>
+    <visual>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <mesh
+          filename="package://stretch_description/meshes/link_gripper_fingertip_left.STL" />
+      </geometry>
+      <material
+        name="">
+        <color
+          rgba="0.250980392156863 0.250980392156863 0.250980392156863 1" />
+      </material>
+    </visual>
+    <collision>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <mesh
+          filename="package://stretch_description/meshes/link_gripper_fingertip_left.STL" />
+      </geometry>
+    </collision>
+  </link>
+  
+  <joint
+    name="joint_gripper_fingertip_left"
+    type="fixed">
+    <origin
+      xyz="${scale_finger_length * 0.19011} 0.014912 0"
+      rpy="-1.5708 -4.774E-15 -2.5545" />
+    <parent
+      link="link_gripper_finger_left" />
+    <child
+      link="link_gripper_fingertip_left" />
+    <axis
+      xyz="0 0 0" />
+  </joint>
+  
+  <link
+    name="link_gripper_finger_right">
+    <inertial>
+      <origin
+        xyz="-0.0945047483510099 -0.0124301080924345 0"
+        rpy="0 0 0" />
+      <mass
+        value="0.0476207785199481" />
+      <inertia
+        ixx="0.001"
+        ixy="0"
+        ixz="0"
+        iyy="0.001"
+        iyz="0"
+        izz="0.001" />
+    </inertial>
+    <visual>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <mesh
+          filename="package://stretch_description/meshes/link_gripper_finger_right.STL" scale="${scale_finger_length} 1.0 1.0"/>
+      </geometry>
+      <material
+        name="">
+        <color
+          rgba="0.792156862745098 0.819607843137255 0.933333333333333 1" />
+      </material>
+    </visual>
+    <collision>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <mesh
+          filename="package://stretch_description/meshes/link_gripper_finger_right.STL" scale="${scale_finger_length} 1.0 1.0"/>
+      </geometry>
+    </collision>
+  </link>
+  
+  <joint
+    name="joint_gripper_finger_right"
+    type="revolute">
+    <origin
+      xyz="-0.047231 0.010049 -0.04679"
+      rpy="3.1416 -0.5236 1.2943E-15" />
+    <parent
+      link="link_gripper" />
+    <child
+      link="link_gripper_finger_right" />
+    <axis
+      xyz="0 0 1" />
+    <limit effort="100" lower="-0.6" upper="0.6" velocity="1.0"/>
+  </joint>
+  
+  <link
+    name="link_gripper_fingertip_right">
+    <inertial>
+      <origin
+        xyz="2.59386303963494E-08 6.70949018566347E-09 0.00812579516130402"
+        rpy="0 0 0" />
+      <mass
+        value="0.00382160037319545" />
+      <inertia
+        ixx="0.001"
+        ixy="0"
+        ixz="0"
+        iyy="0.001"
+        iyz="0"
+        izz="0.001" />
+    </inertial>
+    <visual>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <mesh
+          filename="package://stretch_description/meshes/link_gripper_fingertip_right.STL" />
+      </geometry>
+      <material
+        name="">
+        <color
+          rgba="0.250980392156863 0.250980392156863 0.250980392156863 1" />
+      </material>
+    </visual>
+    <collision>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <mesh
+          filename="package://stretch_description/meshes/link_gripper_fingertip_right.STL" />
+      </geometry>
+    </collision>
+  </link>
+  
+  <joint
+    name="joint_gripper_fingertip_right"
+    type="fixed">
+    <origin
+      xyz="${scale_finger_length * -0.19011} -0.014912 0"
+      rpy="-1.5708 -2.0539E-15 0.58705" />
+    <parent
+      link="link_gripper_finger_right" />
+    <child
+      link="link_gripper_fingertip_right" />
+    <axis
+      xyz="0 0 0" />
+  </joint>
+
+  
+  <link
+      name="link_grasp_center">
+  </link>
+  
+  <joint
+    name="joint_grasp_center"
+    type="fixed">
+    <origin
+      xyz="-0.205478 0 -0.138154"
+      rpy="0 0 3.141579" />
+    <parent
+      link="link_gripper" />
+    <child
+      link="link_grasp_center" />
+  </joint>
+  
+</robot>
diff --git a/stretch_gazebo/urdf/stretch_laser_range_finder.xacro b/stretch_gazebo/urdf/stretch_laser_range_finder.xacro
new file mode 100644
index 0000000..bc89ffd
--- /dev/null
+++ b/stretch_gazebo/urdf/stretch_laser_range_finder.xacro
@@ -0,0 +1,59 @@
+<?xml version="1.0"?>
+<robot xmlns:xacro="http://www.ros.org/wiki/xacro" name="stretch_laser_range_finder">
+  
+  <link
+    name="laser">
+    <inertial>
+      <origin
+        xyz="0 0 -0.000755956127492408"
+        rpy="0 0 0" />
+      <mass
+        value="0.0749979022894495" />
+      <inertia
+        ixx="0"
+        ixy="0"
+        ixz="0"
+        iyy="0"
+        iyz="0"
+        izz="0" />
+    </inertial>
+    <visual>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <mesh
+          filename="package://stretch_description/meshes/laser.STL" />
+      </geometry>
+      <material
+        name="">
+        <color
+          rgba="0.250980392156863 0.250980392156863 0.250980392156863 1" />
+      </material>
+    </visual>
+    <collision>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <mesh
+          filename="package://stretch_description/meshes/laser.STL" />
+      </geometry>
+    </collision>
+  </link>
+  
+  <joint
+    name="joint_laser"
+    type="fixed">
+    <origin
+      xyz="0.004 0 0.1664"
+      rpy="0 0 3.1416" />
+    <parent
+      link="base_link" />
+    <child
+      link="laser" />
+    <axis
+      xyz="0 0 0" />
+  </joint>
+
+</robot>
diff --git a/stretch_gazebo/urdf/stretch_main.xacro b/stretch_gazebo/urdf/stretch_main.xacro
new file mode 100644
index 0000000..93f1597
--- /dev/null
+++ b/stretch_gazebo/urdf/stretch_main.xacro
@@ -0,0 +1,841 @@
+<?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>
+      <origin
+        xyz="-0.11587 0.0019426 0.093621"
+        rpy="0 0 0" />
+      <mass
+        value="1.1912" />
+      <inertia
+        ixx="0.0034667"
+        ixy="-5.0568E-06"
+        ixz="0.00042861"
+        iyy="0.0052744"
+        iyz="-5.766E-05"
+        izz="0.0047945" />
+    </inertial>
+    <visual>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <mesh
+          filename="package://stretch_description/meshes/base_link.STL" />
+      </geometry>
+      <material
+        name="">
+        <color
+          rgba="0.79216 0.81961 0.93333 1" />
+      </material>
+    </visual>
+    <collision>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <mesh
+          filename="package://stretch_description/meshes/base_link.STL" />
+      </geometry>
+    </collision>
+  </link>
+
+  <link
+    name="link_right_wheel">
+    <inertial>
+      <origin
+        xyz="1.1719E-11 2.0783E-11 0.037544"
+        rpy="0 0 0" />
+      <mass
+        value="0.0042721" />
+      <inertia
+        ixx="0.001"
+        ixy="0"
+        ixz="0"
+        iyy="0.001"
+        iyz="0"
+        izz="0.001" />
+    </inertial>
+    <visual>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <mesh
+          filename="package://stretch_description/meshes/link_right_wheel.STL" />
+      </geometry>
+      <material
+        name="">
+        <color
+          rgba="0.79216 0.81961 0.93333 1" />
+      </material>
+    </visual>
+    <collision>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <mesh
+          filename="package://stretch_description/meshes/link_right_wheel.STL" />
+      </geometry>
+    </collision>
+  </link>
+
+  <joint
+    name="joint_right_wheel"
+    type="continuous">
+    <origin
+      xyz="0 -0.17035 0.0508"
+      rpy="-1.5708 1.2717E-16 4.8006E-17" />
+    <parent
+      link="base_link" />
+    <child
+      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
+    name="link_left_wheel">
+    <inertial>
+      <origin
+        xyz="-2.0783E-11 -1.1719E-11 -0.037544"
+        rpy="0 0 0" />
+      <mass
+        value="0.0042721" />
+      <inertia
+        ixx="0.001"
+        ixy="0"
+        ixz="0"
+        iyy="0.001"
+        iyz="0"
+        izz="0.001" />
+    </inertial>
+    <visual>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <mesh
+          filename="package://stretch_description/meshes/link_left_wheel.STL" />
+      </geometry>
+      <material
+        name="">
+        <color
+          rgba="0.79216 0.81961 0.93333 1" />
+      </material>
+    </visual>
+    <collision>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <mesh
+          filename="package://stretch_description/meshes/link_left_wheel.STL" />
+      </geometry>
+    </collision>
+  </link>
+
+  <joint
+    name="joint_left_wheel"
+    type="continuous">
+    <origin
+      xyz="0 0.17035 0.0508"
+      rpy="-1.5708 2.6317E-16 -8.2057E-19" />
+    <parent
+      link="base_link" />
+    <child
+      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>
+      <origin
+        xyz="2.0817E-17 0.7075 -2.7756E-17"
+        rpy="0 0 0" />
+      <mass
+        value="1.8285" />
+      <inertia
+        ixx="0.0709854511954588"
+        ixy="-0.00433428742758457"
+        ixz="-0.000186110788697573"
+        iyy="0.000437922053342648"
+        iyz="-0.00288788257713431"
+        izz="0.071104808501661" />
+    </inertial>
+    <visual>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <mesh
+          filename="package://stretch_description/meshes/link_mast.STL" />
+      </geometry>
+      <material
+        name="">
+        <color
+          rgba="0.79216 0.81961 0.93333 1" />
+      </material>
+    </visual>
+    <collision>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <mesh
+          filename="package://stretch_description/meshes/link_mast.STL" />
+      </geometry>
+    </collision>
+  </link>
+
+  <joint
+    name="joint_mast"
+    type="fixed">
+    <origin
+      xyz="-0.067 0.135 0.0284"
+      rpy="1.5708 0 4.8006E-17" />
+    <parent
+      link="base_link" />
+    <child
+      link="link_mast" />
+    <axis
+      xyz="0 0 0" />
+  </joint>
+
+  <link
+    name="link_lift">
+    <inertial>
+      <origin
+        xyz="-0.031727 0.038403 0.013361"
+        rpy="0 0 0" />
+      <mass
+        value="0.27218" />
+      <inertia
+        ixx="0.00052571"
+        ixy="0.00014899"
+        ixz="-1.9258E-05"
+        iyy="0.00030679"
+        iyz="-6.2451E-06"
+        izz="0.00037324" />
+    </inertial>
+    <visual>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <mesh
+          filename="package://stretch_description/meshes/link_lift.STL" />
+      </geometry>
+      <material
+        name="">
+        <color
+          rgba="0.79216 0.81961 0.93333 1" />
+      </material>
+    </visual>
+    <collision>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <mesh
+          filename="package://stretch_description/meshes/link_lift.STL" />
+      </geometry>
+    </collision>
+  </link>
+
+  <joint
+    name="joint_lift"
+    type="prismatic">
+    <origin
+      xyz="-0.037385 0.1666 0"
+      rpy="-1.5708 1.5708 0" />
+    <parent
+      link="link_mast" />
+    <child
+      link="link_lift" />
+    <axis
+      xyz="0 0 1" />
+  <!-- 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
+    name="link_arm_l4">
+    <inertial>
+      <origin
+        xyz="-1.0146E-06 -1.9719E-05 -0.094738"
+        rpy="0 0 0" />
+      <mass
+        value="0.068095" />
+      <inertia
+        ixx="0.0001256"
+        ixy="-5.6914E-12"
+        ixz="6.0647E-09"
+        iyy="0.0001256"
+        iyz="1.1787E-07"
+        izz="1.1091E-10" />
+    </inertial>
+    <visual>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <mesh
+          filename="package://stretch_description/meshes/link_arm_l4.STL" />
+      </geometry>
+      <material
+        name="">
+        <color
+          rgba="0.79216 0.81961 0.93333 1" />
+      </material>
+    </visual>
+    <collision>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <mesh
+          filename="package://stretch_description/meshes/link_arm_l4.STL" />
+      </geometry>
+    </collision>
+  </link>
+
+  <joint
+    name="joint_arm_l4"
+    type="fixed">
+    <origin
+      xyz="-0.2547 0 0"
+      rpy="1.5708 2.4721E-15 -1.5708" />
+    <parent
+      link="link_lift" />
+    <child
+      link="link_arm_l4" />
+    <axis
+      xyz="0 0 0" />
+  </joint>
+
+  <link
+    name="link_arm_l3">
+    <inertial>
+      <origin
+        xyz="-5.13853606326845E-07 -1.99844969271112E-05 -0.0971104963726614"
+        rpy="0 0 0" />
+      <mass
+        value="0.0628927381893134" />
+      <inertia
+        ixx="0.001"
+        ixy="0"
+        ixz="0"
+        iyy="0.001"
+        iyz="0"
+        izz="0.001" />
+    </inertial>
+    <visual>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <mesh
+          filename="package://stretch_description/meshes/link_arm_l3.STL" />
+      </geometry>
+      <material
+        name="">
+        <color
+          rgba="0.792156862745098 0.819607843137255 0.933333333333333 1" />
+      </material>
+    </visual>
+    <collision>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <mesh
+          filename="package://stretch_description/meshes/link_arm_l3.STL" />
+      </geometry>
+    </collision>
+  </link>
+
+  <joint
+    name="joint_arm_l3"
+    type="prismatic">
+    <origin
+      xyz="0 0 0.013"
+      rpy="7.68831233799385E-30 2.36716479416092E-30 2.29652732251143E-17" />
+    <parent
+      link="link_arm_l4" />
+    <child
+      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>
+
+  <link
+    name="link_arm_l2">
+    <inertial>
+      <origin
+        xyz="-5.17421949435687E-07 -2.02045301450349E-05 -0.0968815475684904"
+        rpy="0 0 0" />
+      <mass
+        value="0.0571386353275368" />
+      <inertia
+        ixx="0.001"
+        ixy="0"
+        ixz="0"
+        iyy="0.001"
+        iyz="0"
+        izz="0.001" />
+    </inertial>
+    <visual>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <mesh
+          filename="package://stretch_description/meshes/link_arm_l2.STL" />
+      </geometry>
+      <material
+        name="">
+        <color
+          rgba="0.792156862745098 0.819607843137255 0.933333333333333 1" />
+      </material>
+    </visual>
+    <collision>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <mesh
+          filename="package://stretch_description/meshes/link_arm_l2.STL" />
+      </geometry>
+    </collision>
+  </link>
+
+  <joint
+    name="joint_arm_l2"
+    type="prismatic">
+    <origin
+      xyz="0 0 0.013"
+      rpy="0 1.57655765344625E-30 -1.66533453693773E-16" />
+    <parent
+      link="link_arm_l3" />
+    <child
+      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>
+
+  <link
+    name="link_arm_l1">
+    <inertial>
+      <origin
+        xyz="-5.257E-07 -2.0482E-05 -0.096543"
+        rpy="0 0 0" />
+      <mass
+        value="0.051382" />
+      <inertia
+        ixx="0.001"
+        ixy="0"
+        ixz="0"
+        iyy="0.001"
+        iyz="0"
+        izz="0.001" />
+    </inertial>
+    <visual>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <mesh
+          filename="package://stretch_description/meshes/link_arm_l1.STL" />
+      </geometry>
+      <material
+        name="">
+        <color
+          rgba="0.79216 0.81961 0.93333 1" />
+      </material>
+    </visual>
+    <collision>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <mesh
+          filename="package://stretch_description/meshes/link_arm_l1.STL" />
+      </geometry>
+    </collision>
+  </link>
+
+  <joint
+    name="joint_arm_l1"
+    type="prismatic">
+    <origin
+      xyz="0 0 0.0129999999999981"
+      rpy="-7.63746778746202E-30 -7.88860905221012E-31 1.11022302462516E-16" />
+    <parent
+      link="link_arm_l2" />
+    <child
+      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>
+
+  <link
+    name="link_arm_l0">
+    <inertial>
+      <origin
+        xyz="0.0270582141286185 -0.00189876414654466 -0.0377809018481181"
+        rpy="0 0 0" />
+      <mass
+        value="0.085003260946398" />
+      <inertia
+        ixx="0.001"
+        ixy="0"
+        ixz="0"
+        iyy="0.001"
+        iyz="0"
+        izz="0.001" />
+    </inertial>
+    <visual>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <mesh
+          filename="package://stretch_description/meshes/link_arm_l0.STL" />
+      </geometry>
+      <material
+        name="">
+        <color
+          rgba="0.792156862745098 0.819607843137255 0.933333333333333 1" />
+      </material>
+    </visual>
+    <collision>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <mesh
+          filename="package://stretch_description/meshes/link_arm_l0.STL" />
+      </geometry>
+    </collision>
+  </link>
+
+  <joint
+    name="joint_arm_l0"
+    type="prismatic">
+    <origin
+      xyz="0 0 -0.0137499999991968"
+      rpy="7.63746778746202E-30 -3.80121128864402E-15 2.62707547767438E-15" />
+    <parent
+      link="link_arm_l1" />
+    <child
+      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>
+
+  <link
+    name="link_wrist_yaw">
+    <inertial>
+      <origin
+        xyz="2.20122392535771E-11 2.9317167880849E-05 -0.018966592644729"
+        rpy="0 0 0" />
+      <mass
+        value="0.0404746907425003" />
+      <inertia
+        ixx="0.001"
+        ixy="0"
+        ixz="0"
+        iyy="0.001"
+        iyz="0"
+        izz="0.001" />
+    </inertial>
+    <visual>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <mesh
+          filename="package://stretch_description/meshes/link_wrist_yaw.STL" />
+      </geometry>
+      <material
+        name="">
+        <color
+          rgba="0.898039215686275 0.917647058823529 0.929411764705882 1" />
+      </material>
+    </visual>
+    <collision>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <mesh
+          filename="package://stretch_description/meshes/link_wrist_yaw.STL" />
+      </geometry>
+    </collision>
+  </link>
+
+  <joint
+    name="joint_wrist_yaw"
+    type="revolute">
+    <origin
+      xyz="0.0830000000000654 -0.0307500000000129 0"
+      rpy="1.5708 4.2595E-14 2.6415E-15"/>
+    <parent
+      link="link_arm_l0" />
+    <child
+      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>
+
+  <link
+    name="link_head">
+    <inertial>
+      <origin
+        xyz="0.0406850995527703 0.0396956343318414 0.0226500246461012"
+        rpy="0 0 0" />
+      <mass
+        value="0.133027236718691" />
+      <inertia
+        ixx="0.001"
+        ixy="0"
+        ixz="0"
+        iyy="0.001"
+        iyz="0"
+        izz="0.001" />
+    </inertial>
+    <visual>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <mesh
+          filename="package://stretch_description/meshes/link_head.STL" />
+      </geometry>
+      <material
+        name="">
+        <color
+          rgba="0.792156862745098 0.819607843137255 0.933333333333333 1" />
+      </material>
+    </visual>
+    <collision>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <mesh
+          filename="package://stretch_description/meshes/link_head.STL" />
+      </geometry>
+    </collision>
+  </link>
+
+  <joint
+    name="joint_head"
+    type="fixed">
+    <origin
+      xyz="0 1.33 0"
+      rpy="1.5707963267949 -1.5707963267949 3.1416" />
+    <parent
+      link="link_mast" />
+    <child
+      link="link_head" />
+    <axis
+      xyz="0 0 0" />
+  </joint>
+
+  <link
+    name="link_head_pan">
+    <inertial>
+      <origin
+        xyz="-0.000326562615178591 0.00850012613776489 0.000130487222982367"
+        rpy="0 0 0" />
+      <mass
+        value="0.0273764496535409" />
+      <inertia
+        ixx="0.001"
+        ixy="0"
+        ixz="0"
+        iyy="0.001"
+        iyz="0"
+        izz="0.001" />
+    </inertial>
+    <visual>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <mesh
+          filename="package://stretch_description/meshes/link_head_pan.STL" />
+      </geometry>
+      <material
+        name="">
+        <color
+          rgba="0.792156862745098 0.819607843137255 0.933333333333333 1" />
+      </material>
+    </visual>
+    <collision>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <mesh
+          filename="package://stretch_description/meshes/link_head_pan.STL" />
+      </geometry>
+    </collision>
+  </link>
+
+  <joint
+    name="joint_head_pan"
+    type="revolute">
+    <origin
+      xyz="0.135 0.0731000000000001 -0.00319621125547975"
+      rpy="0 0 1.5707963267949" />
+    <parent
+      link="link_head" />
+    <child
+      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>
+
+  <link
+    name="link_head_tilt">
+    <inertial>
+      <origin
+        xyz="0.00704566394917504 -0.0212256210929691 0.0302058990060359"
+        rpy="0 0 0" />
+      <mass
+        value="0.090217113313934" />
+      <inertia
+        ixx="0.001"
+        ixy="0"
+        ixz="0"
+        iyy="0.001"
+        iyz="0"
+        izz="0.001" />
+    </inertial>
+    <visual>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <mesh
+          filename="package://stretch_description/meshes/link_head_tilt.STL" />
+      </geometry>
+      <material
+        name="">
+        <color
+          rgba="0.792156862745098 0.819607843137255 0.933333333333333 1" />
+      </material>
+    </visual>
+    <collision>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <mesh
+          filename="package://stretch_description/meshes/link_head_tilt.STL" />
+      </geometry>
+    </collision>
+  </link>
+
+  <joint
+    name="joint_head_tilt"
+    type="revolute">
+    <origin
+      xyz="-0.00130000001785262 0.0277624999926072 -0.0533107920897029"
+      rpy="1.5707963267949 3.36459255518345E-15 -8.42914893687103E-17" />
+    <parent
+      link="link_head_pan" />
+    <child
+      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>
\ No newline at end of file
diff --git a/stretch_gazebo/urdf/stretch_respeaker.xacro b/stretch_gazebo/urdf/stretch_respeaker.xacro
new file mode 100644
index 0000000..120d9ee
--- /dev/null
+++ b/stretch_gazebo/urdf/stretch_respeaker.xacro
@@ -0,0 +1,62 @@
+<?xml version="1.0"?>
+<robot xmlns:xacro="http://www.ros.org/wiki/xacro" name="stretch_respeaker">
+
+  <link
+    name="respeaker_base">
+    <inertial>
+      <origin
+        xyz="-0.003809 -0.0023075 -0.012854"
+        rpy="0 0 0" />
+      <mass
+        value="0.015643" />
+      <inertia
+        ixx="1.0075E-06"
+        ixy="-5.4396E-08"
+        ixz="-2.8652E-07"
+        iyy="1.0569E-06"
+        iyz="-1.8463E-07"
+        izz="1.1947E-07" />
+    </inertial>
+    <visual>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <mesh
+          filename="package://stretch_description/meshes/respeaker_base.STL" />
+      </geometry>
+      <material
+        name="">
+        <color
+          rgba="0.79216 0.81961 0.93333 1" />
+      </material>
+    </visual>
+    <collision>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <mesh
+          filename="package://stretch_description/meshes/respeaker_base.STL" />
+      </geometry>
+    </collision>
+  </link>
+
+  <joint
+    name="joint_respeaker"
+    type="fixed">
+    <origin
+      xyz="0 1.37236408874452 0.00303065898329655"
+      rpy="-1.5707963267949 -0.698131700797725 4.93295812652799E-16" />
+    <parent
+      link="link_mast" />
+    <child
+      link="respeaker_base" />
+    <axis
+      xyz="0 0 0" />
+  </joint>
+  
+</robot>
+
+  
+