增加准备抓取动作

src/brain/config/bt_carry_boxes_sch1.params.yaml

@@ -71,6 +71,20 @@
 
     data_array: [-85, -40, -40, 1, -70, -180, -95, 40, 40, -1, 70, -10]
     
+    '
+- name: s4_arm_prepare_grasp_box
+  params: 'body_id: 0
+
+    data_type: 2
+
+    data_length: 12
+
+    command_id: 0
+
+    frame_time_stamp: 0
+
+    data_array: [-80, -75, 0, 0, -100, -80, -100, 75, 0, 0, 100, -100]
+
     '
 - name: s4_arm_grasp_box
   params: 'body_id: 0
@@ -233,6 +247,20 @@
 
     data_array: [-85, -40, -40, 1, -70, -180, -95, 40, 40, -1, 70, -10]
     
+    '
+- name: s16_arm_prepare_grasp_box
+  params: 'body_id: 0
+
+    data_type: 2
+
+    data_length: 12
+
+    command_id: 0
+
+    frame_time_stamp: 0
+
+    data_array: [-80, -75, 0, 0, -100, -80, -100, 75, 0, 0, 100, -100]
+
     '
 - name: s16_arm_grasp_box
   params: 'body_id: 0

src/brain/config/bt_carry_boxes_sch1.xml

@@ -12,6 +12,7 @@
           </Parallel>
           <Arm_H name="s3_snapshot_action1" />
           <Arm_H name="s3_snapshot_action2" />
+          <Arm_H name="s4_arm_prepare_grasp_box" />
           <Arm_H name="s4_arm_grasp_box" />
           <Arm_H name="s5_arm_pickup_box" />
           <Parallel name="parallel_action_2">
@@ -35,6 +36,7 @@
           </Parallel>
           <Arm_H name="s15_snapshot_action1" />
           <Arm_H name="s15_snapshot_action2" />
+          <Arm_H name="s16_arm_prepare_grasp_box" />
           <Arm_H name="s16_arm_grasp_box" />
           <Arm_H name="s17_arm_pickup_box" />
           <Parallel name="parallel_action_6">