diff --git a/stretch_gazebo/nodes/keyboard_teleop_gazebo b/stretch_gazebo/nodes/keyboard_teleop_gazebo
index d68b6da..57ad692 100755
--- a/stretch_gazebo/nodes/keyboard_teleop_gazebo
+++ b/stretch_gazebo/nodes/keyboard_teleop_gazebo
@@ -1,4 +1,4 @@
-#!/usr/bin/env python
+#!/usr/bin/env python3
from __future__ import print_function
import math
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 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
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 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
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 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
\ 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 @@
-
-
-
-
-
-
+
+
+
+
+
+
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 @@
-
-
-
-
-
-
+
+
+
+
+
+
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 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
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 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
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 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ 0
+ 0
+ 1.0
+ 1.0
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
\ 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 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+