diff --git a/stretch_gazebo/launch/gazebo.launch b/stretch_gazebo/launch/gazebo.launch
index 148ae29..6b75bcd 100644
--- a/stretch_gazebo/launch/gazebo.launch
+++ b/stretch_gazebo/launch/gazebo.launch
@@ -1,15 +1,18 @@
 <launch>
 
-  <arg name="paused" default="false"/>
+  <arg name="paused" default="true"/>
   <arg name="use_sim_time" default="true"/>
   <arg name="gui" default="true"/>
   <arg name="headless" default="false"/>
   <arg name="debug" default="false"/>
   <arg name="rviz" default="false"/>
   <arg name="model" default="$(find stretch_gazebo)/urdf/stretch_gazebo.urdf.xacro"/>
+  <arg name="gpu_lidar" default="false"/>
+  <arg name="visualize_lidar" default="false"/>
+  <arg name="world" default="worlds/empty.world"/>
 
   <include file="$(find gazebo_ros)/launch/empty_world.launch">
-    <arg name="world_name" value="/usr/share/gazebo-11/worlds/willowgarage.world"/>
+    <arg name="world_name" value="$(arg world)" />
     <arg name="debug" value="$(arg debug)" />
     <arg name="gui" value="$(arg gui)" />
     <arg name="paused" value="$(arg paused)"/>
@@ -18,11 +21,11 @@
     <arg name="verbose" value="true"/>
   </include>
 
-  <param name="robot_description" command="$(find xacro)/xacro $(arg model)" />
+  <param name="robot_description" command="$(find xacro)/xacro $(arg model) gpu_lidar:=$(arg gpu_lidar) visualize_lidar:=$(arg visualize_lidar)" />
 
   <!-- push robot_description to factory and spawn robot in gazebo -->
   <node name="urdf_spawner" pkg="gazebo_ros" type="spawn_model"
-        args=" -urdf -model robot -param robot_description" respawn="false" output="screen" />
+        args=" -urdf -model robot -param robot_description -J joint_lift 0.2 -J joint_wrist_yaw 3.14" respawn="false" output="screen" />
 
   <node pkg="robot_state_publisher" type="robot_state_publisher"  name="robot_state_publisher">
     <param name="publish_frequency" type="double" value="30.0" />
diff --git a/stretch_gazebo/scripts/publish_ground_truth_odom.py b/stretch_gazebo/scripts/publish_ground_truth_odom.py
index f6ee321..ac76c62 100755
--- a/stretch_gazebo/scripts/publish_ground_truth_odom.py
+++ b/stretch_gazebo/scripts/publish_ground_truth_odom.py
@@ -3,14 +3,17 @@
 from gazebo_msgs.srv import GetModelState, GetModelStateRequest, GetWorldProperties
 from nav_msgs.msg import Odometry
 from std_msgs.msg import Header
+from std_srvs.srv import Empty
 import rospy
+import time
 
 rospy.init_node('ground_truth_odometry_publisher')
 odom_pub=rospy.Publisher('ground_truth', Odometry, queue_size=10)
 
-rospy.wait_for_service ('/gazebo/get_model_state')
+rospy.wait_for_service('/gazebo/get_model_state')
 get_model_srv = rospy.ServiceProxy('/gazebo/get_model_state', GetModelState)
 get_world_properties = rospy.ServiceProxy('/gazebo/get_world_properties', GetWorldProperties)
+unpause_physics = rospy.ServiceProxy('/gazebo/unpause_physics', Empty)
 
 odom=Odometry()
 header = Header()
@@ -20,6 +23,12 @@ model.model_name='robot'
 models = []
 r = rospy.Rate(20)
 
+pause_timeout = time.time() + 4.0
+while time.time() < pause_timeout:
+    rospy.logwarn("Waiting %.2f seconds to unpause physics", pause_timeout - time.time())
+    time.sleep(1.0)
+unpause_physics()
+
 while not rospy.is_shutdown():
     if model.model_name not in models:
         models = get_world_properties().model_names
diff --git a/stretch_gazebo/urdf/stretch_gazebo.urdf.xacro b/stretch_gazebo/urdf/stretch_gazebo.urdf.xacro
index 57121dc..e7b071d 100644
--- a/stretch_gazebo/urdf/stretch_gazebo.urdf.xacro
+++ b/stretch_gazebo/urdf/stretch_gazebo.urdf.xacro
@@ -1,5 +1,8 @@
 <?xml version="1.0"?>
 <robot xmlns:xacro="http://www.ros.org/wiki/xacro" name="stretch_description">
+  <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" />
@@ -237,39 +240,77 @@
       <material>Gazebo/Green</material>
   </gazebo>
 
-  <!-- LIDAR -->
-  <gazebo reference="laser">
-    <material>Gazebo/Black</material>
-    <sensor type="gpu_ray" name="laser_sensor">
-      <pose>0 0 0 0 0 0</pose>
-      <visualize>false</visualize>
-      <update_rate>5.5</update_rate>
-      <ray>
-        <scan>
-          <horizontal>
-            <samples>2000</samples>
-            <resolution>1</resolution>
-            <min_angle>${-M_PI}</min_angle>
-            <max_angle>${M_PI}</max_angle>
-          </horizontal>
-        </scan>
-        <range>
-          <min>0.5</min>
-          <max>12.0</max>
-          <resolution>0.01</resolution>
-        </range>
-        <noise>
-          <type>gaussian</type>
-          <mean>0.0</mean>
-          <stddev>0.001</stddev>
-        </noise>
-      </ray>
-      <plugin name="gazebo_ros_lidar_controller" filename="libgazebo_ros_gpu_laser.so">
-        <topicName>scan</topicName>
-        <frameName>laser</frameName>
-      </plugin>
-    </sensor>
-  </gazebo>
+  <!-- Non GPU LIDAR -->
+  <xacro:unless value="$(arg gpu_lidar)">
+    <gazebo reference="laser">
+      <material>Gazebo/Black</material>
+      <sensor type="ray" name="laser_sensor">
+        <pose>0 0 0 0 0 0</pose>
+        <visualize>$(arg visualize_lidar)</visualize>
+        <update_rate>5.5</update_rate>
+        <ray>
+          <scan>
+            <horizontal>
+              <samples>2000</samples>
+              <resolution>1</resolution>
+              <min_angle>${-M_PI}</min_angle>
+              <max_angle>${M_PI}</max_angle>
+            </horizontal>
+          </scan>
+          <range>
+            <min>0.08</min>
+            <max>12.0</max>
+            <resolution>0.01</resolution>
+          </range>
+          <noise>
+            <type>gaussian</type>
+            <mean>0.0</mean>
+            <stddev>0.001</stddev>
+          </noise>
+        </ray>
+        <plugin name="gazebo_ros_lidar_controller" filename="libgazebo_ros_laser.so">
+          <topicName>scan</topicName>
+          <frameName>laser</frameName>
+        </plugin>
+      </sensor>
+    </gazebo>
+  </xacro:unless>
+
+  <!-- GPU LIDAR -->
+  <xacro:if value="$(arg gpu_lidar)">
+    <gazebo reference="laser">
+      <material>Gazebo/Black</material>
+      <sensor type="gpu_ray" name="laser_sensor">
+        <pose>0 0 0 0 0 0</pose>
+        <visualize>$(arg visualize_lidar)</visualize>
+        <update_rate>5.5</update_rate>
+        <ray>
+          <scan>
+            <horizontal>
+              <samples>2000</samples>
+              <resolution>1</resolution>
+              <min_angle>${-M_PI}</min_angle>
+              <max_angle>${M_PI}</max_angle>
+            </horizontal>
+          </scan>
+          <range>
+            <min>0.08</min>
+            <max>12.0</max>
+            <resolution>0.01</resolution>
+          </range>
+          <noise>
+            <type>gaussian</type>
+            <mean>0.0</mean>
+            <stddev>0.001</stddev>
+          </noise>
+        </ray>
+        <plugin name="gazebo_ros_lidar_controller" filename="libgazebo_ros_gpu_laser.so">
+          <topicName>scan</topicName>
+          <frameName>laser</frameName>
+        </plugin>
+      </sensor>
+    </gazebo>
+  </xacro:if>
 
   <!-- Base IMU -->
   <gazebo reference="base_link">