Improve joint simulation

Add gazebo_ros_control PID gains
Tuned manually to make movement via the teleop interface behave well
Wheels still unspecified because they cause the robot to collapse
Use effort joint interface for the gripper so that gripper contacts are well simulated

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}
+

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}

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" />

stretch_gazebo/urdf/stretch_gazebo.urdf.xacro

@@ -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>