diff --git a/stretch_description/urdf/stretch_main.xacro b/stretch_description/urdf/stretch_main.xacro
index 6309cde4..dec1cf1a 100644
--- a/stretch_description/urdf/stretch_main.xacro
+++ b/stretch_description/urdf/stretch_main.xacro
@@ -13,7 +13,7 @@
         xyz="-0.109461304328163 -0.000741018909047708 0.0914915269429946"
         rpy="0 0 0" />
       <mass
-        value="1.0723782659782" />
+        value="6.0" />
       <inertia
         ixx="0.00310580907710135"
         ixy="1.5182848191076E-06"
diff --git a/stretch_gazebo/config/arm.yaml b/stretch_gazebo/config/arm.yaml
index 1e179fc5..2bda06e7 100644
--- a/stretch_gazebo/config/arm.yaml
+++ b/stretch_gazebo/config/arm.yaml
@@ -1,5 +1,5 @@
 stretch_arm_controller:
-  type: "position_controllers/JointTrajectoryController"
+  type: "effort_controllers/JointTrajectoryController"
   joints:
     - joint_lift
     - joint_arm_l3
@@ -21,3 +21,11 @@ stretch_arm_controller:
     stop_trajectory_duration: 0.5
     state_publish_rate:  25
     action_monitor_rate: 10
+
+  gains:
+    joint_lift: { p: 500, d: 100, i: 0, i_clamp: 1 }
+    joint_arm_l3: { p: 100, d: 30, i: 0, i_clamp: 1 }
+    joint_arm_l2: { p: 100, d: 30, i: 0, i_clamp: 1 }
+    joint_arm_l1: { p: 100, d: 30, i: 0, i_clamp: 1 }
+    joint_arm_l0: { p: 100, d: 30, i: 0, i_clamp: 1 }
+    joint_wrist_yaw: { p: 100, d: 1, i: 0, i_clamp: 1 }
diff --git a/stretch_gazebo/config/gazebo_ros_control_params.yaml b/stretch_gazebo/config/gazebo_ros_control_params.yaml
new file mode 100644
index 00000000..ca2d722b
--- /dev/null
+++ b/stretch_gazebo/config/gazebo_ros_control_params.yaml
@@ -0,0 +1,14 @@
+gazebo_ros_control/pid_gains:
+  #joint_right_wheel: {p: 10000, i: 0.0, d: 0.4}
+  #joint_left_wheel: {p: 10000, i: 0.0, d: 0.4}
+  joint_lift: {p: 5000, i: 0.0, d: 200}
+  joint_arm_l0: {p: 100, i: 0.0, d: 30}
+  joint_arm_l1: {p: 100, i: 0.0, d: 30}
+  joint_arm_l2: {p: 100, i: 0.0, d: 30}
+  joint_arm_l3: {p: 100, i: 0.0, d: 30}
+  joint_wrist_yaw: {p: 20, i: 5.0, d: 0.4}
+  joint_head_pan: {p: 2, i: 0.0, d: 0.4}
+  joint_head_tilt: {p: 2, i: 0.0, d: 0.4}
+  joint_gripper_finger_left: {p: 0.5, i: 0.0, d: 0.4}
+  joint_gripper_finger_right: {p: 0.5, i: 0.0, d: 0.4}
+
diff --git a/stretch_gazebo/config/gripper.yaml b/stretch_gazebo/config/gripper.yaml
index 9e2442c1..2a82301e 100644
--- a/stretch_gazebo/config/gripper.yaml
+++ b/stretch_gazebo/config/gripper.yaml
@@ -1,5 +1,5 @@
 stretch_gripper_controller:
-  type: "position_controllers/JointTrajectoryController"
+  type: "effort_controllers/JointTrajectoryController"
   joints:
     - joint_gripper_finger_right
     - joint_gripper_finger_left
@@ -13,3 +13,7 @@ stretch_gripper_controller:
     stop_trajectory_duration: 0.5
     state_publish_rate:  25
     action_monitor_rate: 10
+
+  gains:
+    joint_gripper_finger_right: {p: 100, d: 1, i: 0, i_clamp: 1}
+    joint_gripper_finger_left: {p: 100, d: 1, i: 0, i_clamp: 1}
diff --git a/stretch_gazebo/launch/gazebo.launch b/stretch_gazebo/launch/gazebo.launch
index 6b75bcd5..4725dc57 100644
--- a/stretch_gazebo/launch/gazebo.launch
+++ b/stretch_gazebo/launch/gazebo.launch
@@ -33,6 +33,9 @@
 
   <node name="rviz" pkg="rviz" type="rviz" args="-d $(find stretch_gazebo)/config/sim.rviz" if="$(arg rviz)"/>
 
+  <rosparam command="load"
+              file="$(find stretch_gazebo)/config/gazebo_ros_control_params.yaml"/>
+
   <rosparam command="load"
             file="$(find stretch_gazebo)/config/joints.yaml" 
             ns="stretch_joint_state_controller" />
diff --git a/stretch_gazebo/urdf/stretch_gazebo.urdf.xacro b/stretch_gazebo/urdf/stretch_gazebo.urdf.xacro
index 1e516a35..739ca868 100644
--- a/stretch_gazebo/urdf/stretch_gazebo.urdf.xacro
+++ b/stretch_gazebo/urdf/stretch_gazebo.urdf.xacro
@@ -372,7 +372,7 @@
       <mechanicalReduction>1</mechanicalReduction>
     </actuator>
     <joint name="joint_lift">
-      <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
+      <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
     </joint>
   </transmission>
 
@@ -428,7 +428,7 @@
       <mechanicalReduction>1</mechanicalReduction>
     </actuator>
     <joint name="joint_arm_l0">
-      <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
+      <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
     </joint>
   </transmission>
   <transmission name="trans_arm_l1">
@@ -437,7 +437,7 @@
       <mechanicalReduction>1</mechanicalReduction>
     </actuator>
     <joint name="joint_arm_l1">
-      <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
+      <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
     </joint>
   </transmission>
   <transmission name="trans_arm_l2">
@@ -446,7 +446,7 @@
       <mechanicalReduction>1</mechanicalReduction>
     </actuator>
     <joint name="joint_arm_l2">
-      <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
+      <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
     </joint>
   </transmission>
   <transmission name="trans_arm_l3">
@@ -455,7 +455,7 @@
       <mechanicalReduction>1</mechanicalReduction>
     </actuator>
     <joint name="joint_arm_l3">
-      <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
+      <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
     </joint>
   </transmission>
 
@@ -475,7 +475,7 @@
       <mechanicalReduction>1</mechanicalReduction>
     </actuator>
     <joint name="joint_wrist_yaw">
-      <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
+      <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
     </joint>
   </transmission>
 
@@ -585,7 +585,7 @@
       <mechanicalReduction>1</mechanicalReduction>
     </actuator>
     <joint name="joint_gripper_finger_left">
-      <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
+      <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
     </joint>
   </transmission>
   <transmission name="trans_gripper_finger_right">
@@ -594,7 +594,7 @@
       <mechanicalReduction>1</mechanicalReduction>
     </actuator>
     <joint name="joint_gripper_finger_right">
-      <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
+      <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
     </joint>
 </transmission>