Use effort control for all arm joints

Removes most of the instability in simulation behavior when lifting objects

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 }

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>