update sch

src/brain/config/bt_vision_grasp.params.yaml

@@ -79,7 +79,7 @@
 
     '
 - name: s3_right_arm_vision_pre_grasp
-  params: 'body_id: 1
+  params: &pre_grasp 'body_id: 1
 
     data_type: 101
 
@@ -207,4 +207,8 @@
 - name: s15_gripper_open
   params: *gripper_open
 - name: s16_gripper_open
-  params: *gripper_open
+  params: *gripper_open
+- name: s15_arm_pre_grasp
+  params: *pre_grasp
+- name: s16_arm_pre_grasp
+  params: *pre_grasp

src/brain/config/bt_vision_grasp.xml

@@ -33,10 +33,10 @@
           </Sequence>
           
           <!-- Overall recovery if the entire sequence fails -->
-          <Sequence>
+          <!-- <Sequence>
             <SubTree ID="recovery_subtree"/>
             <AlwaysFailure/>
-          </Sequence>
+          </Sequence> -->
         </Fallback>
       </RetryUntilSuccessful>
     </Sequence>
@@ -130,6 +130,7 @@
       <RetryUntilSuccessful name="retry_grasp_recovery" num_attempts="1">
         <Sequence> 
           <GripperCmd_H name="s15_gripper_open" />
+          <Arm_H name="s15_arm_pre_grasp" />
           <Arm_H name="s15_right_arm_pre_cam_take_photo" />
           <ClearDoneInstances_H name="clear_done_instances_grasp" />
         </Sequence>
@@ -142,6 +143,7 @@
       <RetryUntilSuccessful name="retry_putdown_recovery" num_attempts="1">
         <Sequence> 
           <GripperCmd_H name="s16_gripper_open" />
+          <Arm_H name="s16_arm_pre_grasp" />
           <Arm_H name="s16_right_arm_pre_cam_take_photo" />
           <ClearDoneInstances_H name="clear_done_instances_putdown" />
         </Sequence>