Merge engineer code.

engineer_arm_config/config/engineer.srdf

@@ -34,6 +34,7 @@
         <disable_collisions link1="base_link" link2="left_front_wheel" reason="Adjacent"/>
         <disable_collisions link1="base_link" link2="link1" reason="Adjacent"/>
         <disable_collisions link1="base_link" link2="link2" reason="Default"/>
+        <disable_collisions link1="base_link" link2="link3" reason="Never"/>
         <disable_collisions link1="base_link" link2="link5" reason="Default"/>
         <disable_collisions link1="base_link" link2="link6" reason="Default"/>
         <disable_collisions link1="gimbal_base" link2="link1" reason="Never"/>
@@ -50,4 +51,6 @@
         <disable_collisions link1="link6" link2="link7" reason="Never"/>
         <disable_collisions link1="link5" link2="link7" reason="Never"/>
         <disable_collisions link1="link1" link2="camera_link" reason="Never"/>
+        <disable_collisions link1="link7" link2="ore_bin_lifter" reason="Never"/>
+        <disable_collisions link1="link7" link2="ore_bin_rotator" reason="Never"/>
 </robot>

engineer_arm_config/config/kinematics.yaml

@@ -1,4 +1,3 @@
 engineer_arm:
-  kinematics_solver: kdl_kinematics_plugin/KDLKinematicsPlugin
-  kinematics_solver_search_resolution: 0.005
+  kinematics_solver: trac_ik_kinematics_plugin/TRAC_IKKinematicsPlugin
   kinematics_solver_timeout: 0.005

engineer_arm_config/launch/load_controllers.launch

@@ -32,6 +32,10 @@
                 controllers/gpio_controller
                 controllers/gimbal_controller
                 controllers/ore_bin_rotate_controller
+                controllers/extend_arm_front_controller
+                controllers/extend_arm_back_controller
+                controllers/extend_arm_front_calibration_controller
+                controllers/extend_arm_back_calibration_controller
                 controllers/ore_bin_lifter_controller
                 controllers/ore_bin_lifter_calibration_controller
                 controllers/gimbal_lifter_controller

engineer_middleware/config/engineer.yaml

@@ -1,9 +1,9 @@
 chassis:
   x:
-    pid: { p: 0.0, i: 0.0, d: 0.0, i_clamp_max: 0, i_clamp_min: 0, antiwindup: true, publish_state: true }
+    pid: { p: 4.5, i: 0.0, d: 0.2, i_clamp_max: 0, i_clamp_min: 0, antiwindup: true, publish_state: true }
   y:
-    pid: { p: 0.0, i: 0.0, d: 0.0, i_clamp_max: 0, i_clamp_min: 0, antiwindup: true, publish_state: true }
+    pid: { p: 4.5, i: 0.0, d: 0.2, i_clamp_max: 0, i_clamp_min: 0, antiwindup: true, publish_state: true }
   yaw:
-    pid: { p: 3., i: 0.0, d: 0.1, i_clamp_max: 0, i_clamp_min: 0, antiwindup: true, publish_state: true }
+    pid: { p: 5., i: 0.0, d: 0.05, i_clamp_max: 0, i_clamp_min: 0, antiwindup: true, publish_state: true }
   yaw_start_threshold: 0.05
   max_vel: 0.1

engineer_middleware/config/simulation_controllers.yaml

@@ -114,12 +114,12 @@ controllers:
     joint: gimbal_lifter_joint
     pid: { p: 20000., i: 0., d: 1200, i_max: 0.01, i_min: 0.01, antiwindup: true, publish_state: true }
 
-  extend_arm_a_controller:
+  extend_arm_front_controller:
     type: effort_controllers/JointPositionController
-    joint: extend_arm_a_joint
+    joint: extend_arm_front_joint
     pid: { p: 3., i: 0.1, d: 0.05, i_max: 0.01, i_min: 0.01, antiwindup: true, publish_state: true }
 
-  extend_arm_b_controller:
+  extend_arm_back_controller:
     type: effort_controllers/JointPositionController
-    joint: extend_arm_b_joint
+    joint: extend_arm_back_joint
     pid: { p: 3., i: 0.1, d: 0.05, i_max: 0.01, i_min: 0.01, antiwindup: true, publish_state: true }