update dual arm demo1 params

src/brain/config/bt_vision_grasp_dual_arm_demo1.params.yaml

@@ -25,7 +25,7 @@
 
     frame_time_stamp: 0
 
-    data_array: [0, 0, 0, 0, 0, 0, 95, -22, -90, 0, -68, 90]
+    data_array: [0, 0, 0, 0, 0, 0, 80, -5, -112, -12, -62, 175]
     
     '
 - name: s1_left_arm_initial
@@ -39,7 +39,7 @@
 
     frame_time_stamp: 0
 
-    data_array: [85, 22, 90, 0, 68, 90, 0, 0, 0, 0, 0, 0]
+    data_array: [100, 5, 112, 12, 62, 5, 0, 0, 0, 0, 0, 0]
     
     '
 - name: s1_right_arm_pre_cam_take_photo
@@ -53,7 +53,7 @@
 
     frame_time_stamp: 0
 
-    data_array: [0, 0, 0, 0, 0, 0, 60, -22, -90, 0, -68, 90]
+    data_array: [0, 0, 0, 0, 0, 0, 14.29, -34.07, -122.53, -76.89, 84.40, 112.89]
     
     '
 - name: s1_left_arm_pre_cam_take_photo
@@ -67,7 +67,7 @@
 
     frame_time_stamp: 0
 
-    data_array: [120, 22, 90, 0, 68, 90, 0, 0, 0, 0, 0, 0]
+    data_array: [165.80, 34.07, 122.53, 76.89, -84.40, 67.20, 0, 0, 0, 0, 0, 0]
     
     '
 - name: s2_right_arm_cam_take_photo
@@ -393,6 +393,6 @@
 - name: s10_wheel_move_to_origin_pickup_position
   params: *origin_pickup_position
 - name: s15_left_arm_cam_take_photo
-  params: &left_arm_cam_take_photo
+  params: *left_arm_cam_take_photo
 - name: s15_right_arm_cam_take_photo
-  params: &right_arm_cam_take_photo
+  params: *right_arm_cam_take_photo

src/brain/config/bt_vision_grasp_dual_arm_demo1.xml

@@ -67,8 +67,8 @@
       <Parallel name="parallel_init_action">
         <GripperCmd1_H name="s1_right_hand_gripper_open" />
         <GripperCmd0_H name="s1_left_hand_gripper_open" />
-        <Arm_H name="s1_right_arm_pre_cam_take_photo" />
-        <Arm_H name="s1_left_arm_pre_cam_take_photo" />
+        <!-- <Arm_H name="s1_right_arm_pre_cam_take_photo" />
+        <Arm_H name="s1_left_arm_pre_cam_take_photo" /> -->
       </Parallel>
       <RetryUntilSuccessful name="retry_top_cam_vision" num_attempts="5">
         <VisionObjectRecognition_H name="s2_top_camera_vision_recg" />
@@ -296,6 +296,10 @@
           <SubTree ID="right_arm_grasp_subtree"/>
           <Script code="right_done:=true" />
         </Sequence>
+        <Sequence>
+          <ScriptCondition code="right_v_ok==true" />
+          <SubTree ID="right_arm_grasp_recovery_subtree"/>
+        </Sequence>
         <AlwaysSuccess/>
       </Fallback>
       <!-- 左臂抓取 -->
@@ -305,6 +309,10 @@
           <SubTree ID="left_arm_grasp_subtree"/>
           <Script code="left_done:=true" />
         </Sequence>
+        <Sequence>
+          <ScriptCondition code="left_v_ok==true" />
+          <SubTree ID="left_arm_grasp_recovery_subtree"/>
+        </Sequence>
         <AlwaysSuccess/>
       </Fallback>
     </Parallel>