Use a single controller for the simulation

This better matches the physical robot, though this interface doesn't handle the virtual joints that the robot uses

stretch_gazebo/config/arm.yaml

@@ -1,31 +0,0 @@
-stretch_arm_controller:
-  type: "effort_controllers/JointTrajectoryController"
-  joints:
-    - joint_lift
-    - joint_arm_l3
-    - joint_arm_l2
-    - joint_arm_l1
-    - joint_arm_l0
-    - joint_wrist_yaw
-  allow_partial_joints_goal: true
-
-  constraints:
-    goal_time: 0.6
-    stopped_velocity_tolerance: 0.05
-    joint_lift: {trajectory: 0.1, goal: 0.1}
-    joint_arm_l3: {trajectory: 0.1, goal: 0.1}
-    joint_arm_l2: {trajectory: 0.1, goal: 0.1}
-    joint_arm_l1: {trajectory: 0.1, goal: 0.1}
-    joint_arm_l0: {trajectory: 0.1, goal: 0.1}
-    joint_wrist_yaw: {trajectory: 0.1, goal: 0.1}
-    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/config/body.yaml

@@ -0,0 +1,35 @@
+stretch_controller_raw:
+  type: "effort_controllers/JointTrajectoryController"
+  joints:
+    - joint_head_pan
+    - joint_head_tilt
+    - joint_lift
+    - joint_arm_l3
+    - joint_arm_l2
+    - joint_arm_l1
+    - joint_arm_l0
+    - joint_wrist_yaw
+    - joint_gripper_finger_right
+    - joint_gripper_finger_left
+  allow_partial_joints_goal: true
+
+  constraints:
+    goal_time: 0.6
+    stopped_velocity_tolerance: 0.05
+
+    stop_trajectory_duration: 0.5
+    state_publish_rate:  25
+    action_monitor_rate: 10
+
+
+  gains:
+    joint_head_pan: {p: 10, d: 1, i: 0, i_clamp: 1}
+    joint_head_tilt: {p: 10, d: 1, i: 0, i_clamp: 1}
+    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}
+    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/config/gripper.yaml

@@ -1,19 +0,0 @@
-stretch_gripper_controller:
-  type: "effort_controllers/JointTrajectoryController"
-  joints:
-    - joint_gripper_finger_right
-    - joint_gripper_finger_left
-  allow_partial_joints_goal: true
-
-  constraints:
-    goal_time: 0.6
-    stopped_velocity_tolerance: 0.05
-    joint_gripper_finger_right: {trajectory: 0.1, goal: 0.1}
-    joint_gripper_finger_left: {trajectory: 0.1, goal: 0.1}
-    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/config/head.yaml

@@ -1,15 +0,0 @@
-stretch_head_controller:
-  type: "position_controllers/JointTrajectoryController"
-  joints:
-    - joint_head_pan
-    - joint_head_tilt
-  allow_partial_joints_goal: true
-
-  constraints:
-    goal_time: 0.6
-    stopped_velocity_tolerance: 0.05
-    joint_head_pan: {trajectory: 0.1, goal: 0.1}
-    joint_head_tilt: {trajectory: 0.1, goal: 0.1}
-    stop_trajectory_duration: 0.5
-    state_publish_rate:  25
-    action_monitor_rate: 10

stretch_gazebo/launch/gazebo.launch

@@ -45,16 +45,10 @@
             ns="stretch_diff_drive_controller" />
 
   <rosparam command="load"
-            file="$(find stretch_gazebo)/config/arm.yaml"/>
-
-  <rosparam command="load"
-            file="$(find stretch_gazebo)/config/head.yaml" />
-
-  <rosparam command="load"
-            file="$(find stretch_gazebo)/config/gripper.yaml" />
+            file="$(find stretch_gazebo)/config/body.yaml" />
 
   <node name="stretch_controller_spawner" pkg="controller_manager" type="spawner"
-        args="stretch_joint_state_controller stretch_diff_drive_controller stretch_arm_controller stretch_head_controller stretch_gripper_controller"/>
+        args="stretch_joint_state_controller stretch_diff_drive_controller stretch_controller_raw"/>
 
   <node name="publish_ground_truth_odom" pkg="stretch_gazebo" type="publish_ground_truth_odom.py" output="screen"/>
 

stretch_gazebo/urdf/stretch_gazebo.urdf.xacro

@@ -510,7 +510,7 @@
       <mechanicalReduction>1</mechanicalReduction>
     </actuator>
     <joint name="joint_head_pan">
-      <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
+      <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
     </joint>
   </transmission>
 
@@ -520,7 +520,7 @@
       <mechanicalReduction>1</mechanicalReduction>
     </actuator>
     <joint name="joint_head_tilt">
-      <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
+      <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
     </joint>
   </transmission>