HiveCoreRD/hivecore_robot_brain
6
0
Fork 0
Code Issues Pull Requests 1 Actions Packages Projects Releases Wiki Activity

optimize code

Browse Source
This commit is contained in:
NuoDaJia02
2026-01-12 17:55:58 +08:00
parent 88dceb1a1a
commit 2d4a1edb10
9 changed files with 784 additions and 30 deletions
Download Patch File Download Diff File Expand all files Collapse all files

12
src/brain/config/bt_vision_grasp_dual_arm.params.yaml
View File

@@ -71,7 +71,7 @@
'
- name: s2_right_arm_cam_take_photo
params: 'body_id: 1
params: &right_arm_cam_take_photo 'body_id: 1
data_type: 100
@@ -85,7 +85,7 @@
'
- name: s2_left_arm_cam_take_photo
params: 'body_id: 0
params: &left_arm_cam_take_photo 'body_id: 0
data_type: 100
@@ -173,7 +173,7 @@
torque: 100
mode: 1
mode: 2
'
- name: s4_left_hand_gripper_close
@@ -391,4 +391,8 @@
'
- name: s10_wheel_move_to_origin_pickup_position
params: *origin_pickup_position
params: *origin_pickup_position
- name: s15_left_arm_cam_take_photo
params: *left_arm_cam_take_photo
- name: s15_right_arm_cam_take_photo
params: *right_arm_cam_take_photo

23
src/brain/config/bt_vision_grasp_dual_arm.xml
View File

@@ -5,9 +5,6 @@
<RetryUntilSuccessful name="retry_vision_grasp" num_attempts="3">
<Fallback name="fallback_main_action">
<Sequence name="complete_pick_and_place">
<!-- Phase 0: Move Wheel to Origin Pickup Position -->
<MoveWheel_H name="s1_wheel_move_to_origin_pickup_position" />
<!-- Phase 1: Shared Vision Setup (Top Camera) -->
<Fallback name="shared_vision_with_recovery">
<SubTree ID="vision_setup_subtree"/>
@@ -26,19 +23,11 @@
<!-- Phase 3: Independent Dual Arm Operation -->
<SubTree ID="dual_arm_grasp_subtree_wrapper" right_v_ok="{right_vision_ok}" left_v_ok="{left_vision_ok}" right_done="{right_arm_done}" left_done="{left_arm_done}"/>
<Parallel name="parallel_action_2">
<MoveWheel_H name="s6_wheel_move_to_putdown_position" />
<MoveWaist_H name="s6_move_waist_turn_left_90" />
</Parallel>
<!-- Phase 4: Dual Arm Putdown -->
<SubTree ID="dual_arm_putdown_subtree_wrapper" right_done="{right_arm_done}" left_done="{left_arm_done}"/>
<MoveWaist_H name="s10_move_waist_turn_right_90" />
<!-- Phase 5: Dual Arm Initialization -->
<!-- Phase 5: Dual Arm Hand Vision -->
<Parallel name="dual_arm_init_action">
<MoveWheel_H name="s10_wheel_move_to_origin_pickup_position" />
<Arm_H name="s11_right_arm_initial" />
<Arm_H name="s11_left_arm_initial" />
</Parallel>
@@ -177,6 +166,7 @@
<Sequence name="right_grasp_recovery_root">
<GripperCmd1_H name="s15_right_hand_gripper_open" />
<Arm_H name="s15_right_arm_pre_grasp" />
<Arm_H name="s15_right_arm_cam_take_photo" />
<Arm_H name="s15_right_arm_initial" />
<ClearDoneInstances_H name="clear_done_instances_right_grasp" />
</Sequence>
@@ -186,6 +176,7 @@
<Sequence name="left_grasp_recovery_root">
<GripperCmd0_H name="s15_left_hand_gripper_open" />
<Arm_H name="s15_left_arm_pre_grasp" />
<Arm_H name="s15_left_arm_cam_take_photo" />
<Arm_H name="s15_left_arm_initial" />
<ClearDoneInstances_H name="clear_done_instances_left_grasp" />
</Sequence>
@@ -294,6 +285,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>
<!-- 左臂抓取 -->
@@ -303,6 +298,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>

397
src/brain/config/bt_vision_grasp_dual_arm_demo1.params.yaml Normal file
View File

@@ -0,0 +1,397 @@
- name: root
params: '{}
'
- name: s1_right_hand_gripper_open
params: &gripper_open 'loc: 0
speed: 150
torque: 20
mode: 2
'
- name: s1_left_hand_gripper_open
params: *gripper_open
- name: s1_right_arm_initial
params: &arm_inittial_right 'body_id: 1
data_type: 2
data_length: 12
command_id: 0
frame_time_stamp: 0
data_array: [0, 0, 0, 0, 0, 0, 95, -25, -90, 0, -68, 90]
'
- name: s1_left_arm_initial
params: &arm_inittial_left 'body_id: 0
data_type: 2
data_length: 12
command_id: 0
frame_time_stamp: 0
data_array: [85, 25, 90, 0, 68, 90, 0, 0, 0, 0, 0, 0]
'
- name: s1_right_arm_pre_cam_take_photo
params: 'body_id: 1
data_type: 2
data_length: 12
command_id: 0
frame_time_stamp: 0
data_array: [0, 0, 0, 0, 0, 0, 60, -20, -80, 0, -80, 60]
'
- name: s1_left_arm_pre_cam_take_photo
params: 'body_id: 0
data_type: 2
data_length: 12
command_id: 0
frame_time_stamp: 0
data_array: [120, 20, 80, 0, 80, 120, 0, 0, 0, 0, 0, 0]
'
- name: s2_right_arm_cam_take_photo
params: &right_arm_cam_take_photo 'body_id: 1
data_type: 100
data_length: 0
command_id: 0
frame_time_stamp: 0
data_array: []
'
- name: s2_left_arm_cam_take_photo
params: &left_arm_cam_take_photo 'body_id: 0
data_type: 100
data_length: 0
command_id: 0
frame_time_stamp: 0
data_array: []
'
- name: s2_top_camera_vision_recg
params: 'camera_position: top
'
- name: s3_right_camera_vision_recg
params: 'camera_position: right
'
- name: s3_left_camera_vision_recg
params: 'camera_position: left
'
- name: s3_right_arm_vision_pre_grasp
params: &pre_grasp_right 'body_id: 1
data_type: 101
data_length: 0
command_id: 0
frame_time_stamp: 0
data_array: []
'
- name: s3_left_arm_vision_pre_grasp
params: &pre_grasp_left 'body_id: 0
data_type: 101
data_length: 0
command_id: 0
frame_time_stamp: 0
data_array: []
'
- name: s4_right_arm_vision_grasp
params: 'body_id: 1
data_type: 102
data_length: 0
command_id: 0
frame_time_stamp: 0
data_array: []
'
- name: s4_left_arm_vision_grasp
params: 'body_id: 0
data_type: 102
data_length: 0
command_id: 0
frame_time_stamp: 0
data_array: []
'
- name: s4_right_hand_gripper_close
params: &gripper_close 'loc: 200
speed: 150
torque: 100
mode: 1
'
- name: s4_left_hand_gripper_close
params: *gripper_close
- name: s4_right_arm_take_box
params: 'body_id: 1
data_type: 110
data_length: 0
command_id: 0
frame_time_stamp: 0
data_array: []
'
- name: s4_left_arm_take_box
params: 'body_id: 0
data_type: 110
data_length: 0
command_id: 0
frame_time_stamp: 0
data_array: []
'
- name: s5_right_arm_take_box_off
params: 'body_id: 1
data_type: 2
data_length: 12
command_id: 0
frame_time_stamp: 0
data_array: [0, 0, 0, 0, 0, 0, 96.31, -103.06, 17.69, -53.40, -7.81, 143.19]
'
- name: s5_left_arm_take_box_off
params: 'body_id: 0
data_type: 2
data_length: 12
command_id: 0
frame_time_stamp: 0
data_array: [83.78, 103.06, -17.69, 53.40, 7.81, 36.90, 0, 0, 0, 0, 0, 0]
'
- name: s7_right_arm_grasp_box
params: &grasp_box_s7_s9_right 'body_id: 1
data_type: 2
data_length: 12
command_id: 0
frame_time_stamp: 0
data_array: [0, 0, 0, 0, 0, 0, 96.31, -103.06, 17.69, -53.40, -7.81, 143.19]
'
- name: s7_left_arm_grasp_box
params: &grasp_box_s7_s9_left 'body_id: 0
data_type: 2
data_length: 12
command_id: 0
frame_time_stamp: 0
data_array: [83.78, 103.06, -17.69, 53.40, 7.81, 36.90, 0, 0, 0, 0, 0, 0]
'
- name: s7_right_arm_put_down_box
params: 'body_id: 1
data_type: 2
data_length: 12
command_id: 0
frame_time_stamp: 0
data_array: [0, 0, 0, 0, 0, 0, 85.28, -103.86, 19.46, 40.34, -7.37, 49.94]
'
- name: s7_left_arm_put_down_box
params: 'body_id: 0
data_type: 2
data_length: 12
command_id: 0
frame_time_stamp: 0
data_array: [94.81, 103.86, -19.46, -40.33, 7.37, 130.15, 0, 0, 0, 0, 0, 0]
'
- name: s8_right_hand_gripper_open
params: *gripper_open
- name: s8_left_hand_gripper_open
params: *gripper_open
- name: s9_right_arm_grasp_box
params: *grasp_box_s7_s9_right
- name: s9_left_arm_grasp_box
params: *grasp_box_s7_s9_left
- name: s11_right_arm_initial
params: *arm_inittial_right
- name: s11_left_arm_initial
params: *arm_inittial_left
- name: s12_right_hand_gripper_open
params: *gripper_open
- name: s12_left_hand_gripper_open
params: *gripper_open
- name: s13_right_arm_initial
params: *arm_inittial_right
- name: s13_left_arm_initial
params: *arm_inittial_left
- name: clear_done_instances_overall
params: '{}
'
- name: s14_right_arm_initial
params: *arm_inittial_right
- name: s14_left_arm_initial
params: *arm_inittial_left
- name: clear_done_instances_vision
params: '{}
'
- name: clear_done_instances_right_grasp
params: '{}
'
- name: clear_done_instances_left_grasp
params: '{}
'
- name: s15_right_hand_gripper_open
params: *gripper_open
- name: s15_left_hand_gripper_open
params: *gripper_open
- name: s15_right_arm_pre_grasp
params: *pre_grasp_right
- name: s15_left_arm_pre_grasp
params: *pre_grasp_left
- name: s15_right_arm_initial
params: *arm_inittial_right
- name: s15_left_arm_initial
params: *arm_inittial_left
- name: s16_right_arm_initial
params: *arm_inittial_right
- name: s16_left_arm_initial
params: *arm_inittial_left
- name: clear_done_instances_arm_vision
params: '{}
'
- name: s17_right_arm_initial
params: *arm_inittial_right
- name: s17_left_arm_initial
params: *arm_inittial_left
- name: clear_done_instances_left_arm_vision
params: '{}
'
- name: clear_done_instances_right_arm_vision
params: '{}
'
- name: s0_right_hand_gripper_open
params: *gripper_open
- name: s0_left_hand_gripper_open
params: *gripper_open
- name: s1_wheel_move_to_origin_pickup_position
params: &origin_pickup_position 'move_distance: 0.0
move_angle: 0.0
'
- name: s6_wheel_move_to_putdown_position
params: 'move_distance: -0.5
move_angle: 0.0
'
- name: s6_move_waist_turn_left_90
params: 'move_pitch_degree: 0.0
move_yaw_degree: 90.0
'
- name: s10_move_waist_turn_right_90
params: &waist_turn_right_90 'move_pitch_degree: 0.0
move_yaw_degree: -90.0
'
- 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
- name: s15_right_arm_cam_take_photo
params: &right_arm_cam_take_photo

341
src/brain/config/bt_vision_grasp_dual_arm_demo1.xml Normal file
View File

@@ -0,0 +1,341 @@
<root BTCPP_format="4" main_tree_to_execute="MainTree">
<BehaviorTree ID="MainTree">
<Sequence name="root">
<Script code="right_arm_cam_take_photo:=false; left_arm_cam_take_photo:=false; right_arm_done:=false; left_arm_done:=false; right_vision_ok:=false; left_vision_ok:=false" />
<RetryUntilSuccessful name="retry_vision_grasp" num_attempts="3">
<Fallback name="fallback_main_action">
<Sequence name="complete_pick_and_place">
<!-- Phase 0: Move Wheel to Origin Pickup Position -->
<MoveWheel_H name="s1_wheel_move_to_origin_pickup_position" />
<!-- Phase 1: Shared Vision Setup (Top Camera) -->
<Fallback name="shared_vision_with_recovery">
<SubTree ID="vision_setup_subtree"/>
<Sequence>
<SubTree ID="vision_recovery_subtree"/>
<AlwaysFailure/>
</Sequence>
</Fallback>
<!-- Move both arms to positions where their hand cameras can see potential objects -->
<SubTree ID="dual_arm_cam_subtree" right_cam_status="{right_arm_cam_take_photo}" left_cam_status="{left_arm_cam_take_photo}"/>
<!-- Phase 2: Sequential Hand Vision -->
<SubTree ID="sequential_hand_vision_subtree" right_done="{right_arm_done}" left_done="{left_arm_done}" right_cam_ok="{right_arm_cam_take_photo}" left_cam_ok="{left_arm_cam_take_photo}" right_v_ok="{right_vision_ok}" left_v_ok="{left_vision_ok}"/>
<!-- Phase 3: Independent Dual Arm Operation -->
<SubTree ID="dual_arm_grasp_subtree_wrapper" right_v_ok="{right_vision_ok}" left_v_ok="{left_vision_ok}" right_done="{right_arm_done}" left_done="{left_arm_done}"/>
<Parallel name="parallel_action_2">
<MoveWheel_H name="s6_wheel_move_to_putdown_position" />
<MoveWaist_H name="s6_move_waist_turn_left_90" />
</Parallel>
<!-- Phase 4: Dual Arm Putdown -->
<SubTree ID="dual_arm_putdown_subtree_wrapper" right_done="{right_arm_done}" left_done="{left_arm_done}"/>
<MoveWaist_H name="s10_move_waist_turn_right_90" />
<!-- Phase 5: Dual Arm Initialization -->
<Parallel name="dual_arm_init_action">
<MoveWheel_H name="s10_wheel_move_to_origin_pickup_position" />
<Arm_H name="s11_right_arm_initial" />
<Arm_H name="s11_left_arm_initial" />
</Parallel>
</Sequence>
<Sequence>
<!-- Overall fallback recovery -->
<SubTree ID="recovery_subtree"/>
<AlwaysFailure/>
</Sequence>
</Fallback>
</RetryUntilSuccessful>
</Sequence>
</BehaviorTree>
<!-- Shared setup and head-camera vision -->
<BehaviorTree ID="vision_setup_subtree">
<Sequence name="vision_setup_root">
<Parallel name="parallel_arm_init_action">
<GripperCmd1_H name="s0_right_hand_gripper_open" />
<GripperCmd0_H name="s0_left_hand_gripper_open" />
<Arm_H name="s1_right_arm_initial" />
<Arm_H name="s1_left_arm_initial" />
</Parallel>
<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" />
</Parallel>
<RetryUntilSuccessful name="retry_top_cam_vision" num_attempts="5">
<VisionObjectRecognition_H name="s2_top_camera_vision_recg" />
</RetryUntilSuccessful>
</Sequence>
</BehaviorTree>
<!-- Right Arm Specific Subtrees -->
<BehaviorTree ID="right_arm_hand_vision_subtree">
<Sequence name="right_hand_vision_root">
<RetryUntilSuccessful name="retry_right_hand_cam_vision" num_attempts="5">
<VisionObjectRecognition_H name="s3_right_camera_vision_recg" />
</RetryUntilSuccessful>
</Sequence>
</BehaviorTree>
<BehaviorTree ID="right_arm_grasp_subtree">
<Sequence name="right_arm_grasp_root">
<RetryUntilSuccessful name="retry_right_arm_grasp_action" num_attempts="1">
<Sequence>
<Arm_H name="s3_right_arm_vision_pre_grasp" />
<Arm_H name="s4_right_arm_vision_grasp" />
<GripperCmd1_H name="s4_right_hand_gripper_close" />
<Arm_H name="s4_right_arm_take_box" />
<Arm_H name="s5_right_arm_take_box_off" />
</Sequence>
</RetryUntilSuccessful>
</Sequence>
</BehaviorTree>
<BehaviorTree ID="right_arm_putdown_subtree">
<Sequence name="right_arm_putdown_root">
<RetryUntilSuccessful name="retry_right_arm_putdown" num_attempts="1">
<Sequence>
<!-- <Arm_H name="s7_right_arm_grasp_box" /> -->
<Arm_H name="s7_right_arm_put_down_box" />
<GripperCmd1_H name="s8_right_hand_gripper_open" />
<Arm_H name="s9_right_arm_grasp_box" />
<!-- <Arm_H name="s11_right_arm_initial" /> -->
</Sequence>
</RetryUntilSuccessful>
</Sequence>
</BehaviorTree>
<!-- Left Arm Specific Subtrees -->
<BehaviorTree ID="left_arm_hand_vision_subtree">
<Sequence name="left_hand_vision_root">
<RetryUntilSuccessful name="retry_left_hand_cam_vision" num_attempts="5">
<VisionObjectRecognition_H name="s3_left_camera_vision_recg" />
</RetryUntilSuccessful>
</Sequence>
</BehaviorTree>
<BehaviorTree ID="left_arm_grasp_subtree">
<Sequence name="left_arm_grasp_root">
<RetryUntilSuccessful name="retry_left_arm_grasp_action" num_attempts="1">
<Sequence>
<Arm_H name="s3_left_arm_vision_pre_grasp" />
<Arm_H name="s4_left_arm_vision_grasp" />
<GripperCmd0_H name="s4_left_hand_gripper_close" />
<Arm_H name="s4_left_arm_take_box" />
<Arm_H name="s5_left_arm_take_box_off" />
</Sequence>
</RetryUntilSuccessful>
</Sequence>
</BehaviorTree>
<BehaviorTree ID="left_arm_putdown_subtree">
<Sequence name="left_arm_putdown_root">
<RetryUntilSuccessful name="retry_left_arm_putdown" num_attempts="1">
<Sequence>
<!-- <Arm_H name="s7_left_arm_grasp_box" /> -->
<Arm_H name="s7_left_arm_put_down_box" />
<GripperCmd0_H name="s8_left_hand_gripper_open" />
<Arm_H name="s9_left_arm_grasp_box" />
<!-- <Arm_H name="s11_left_arm_initial" /> -->
</Sequence>
</RetryUntilSuccessful>
</Sequence>
</BehaviorTree>
<!-- Recovery Subtrees -->
<BehaviorTree ID="recovery_subtree">
<Sequence name="recovery_root">
<Parallel name="overall_recovery_parallel">
<GripperCmd1_H name="s12_right_hand_gripper_open" />
<GripperCmd0_H name="s12_left_hand_gripper_open" />
<Arm_H name="s13_right_arm_initial" />
<Arm_H name="s13_left_arm_initial" />
</Parallel>
<ClearDoneInstances_H name="clear_done_instances_overall" />
</Sequence>
</BehaviorTree>
<BehaviorTree ID="vision_recovery_subtree">
<Sequence name="vision_recovery_root">
<Parallel name="arm_initial2">
<Arm_H name="s14_right_arm_initial" />
<Arm_H name="s14_left_arm_initial" />
</Parallel>
<ClearDoneInstances_H name="clear_done_instances_vision" />
</Sequence>
</BehaviorTree>
<BehaviorTree ID="right_arm_grasp_recovery_subtree">
<Sequence name="right_grasp_recovery_root">
<GripperCmd1_H name="s15_right_hand_gripper_open" />
<Arm_H name="s15_right_arm_pre_grasp" />
<Arm_H name="s15_right_arm_cam_take_photo" />
<Arm_H name="s15_right_arm_initial" />
<ClearDoneInstances_H name="clear_done_instances_right_grasp" />
</Sequence>
</BehaviorTree>
<BehaviorTree ID="left_arm_grasp_recovery_subtree">
<Sequence name="left_grasp_recovery_root">
<GripperCmd0_H name="s15_left_hand_gripper_open" />
<Arm_H name="s15_left_arm_pre_grasp" />
<Arm_H name="s15_left_arm_cam_take_photo" />
<Arm_H name="s15_left_arm_initial" />
<ClearDoneInstances_H name="clear_done_instances_left_grasp" />
</Sequence>
</BehaviorTree>
<BehaviorTree ID="arm_vision_recovery_subtree">
<Sequence name="arm_vision_recovery_root">
<Parallel name="arm_initial3">
<Arm_H name="s16_right_arm_initial" />
<Arm_H name="s16_left_arm_initial" />
</Parallel>
<ClearDoneInstances_H name="clear_done_instances_arm_vision" />
</Sequence>
</BehaviorTree>
<BehaviorTree ID="left_arm_vision_recovery_subtree">
<Sequence name="left_arm_vision_recovery_root">
<Parallel name="left_arm_initial">
<Arm_H name="s17_left_arm_initial" />
</Parallel>
<ClearDoneInstances_H name="clear_done_instances_left_arm_vision" />
</Sequence>
</BehaviorTree>
<BehaviorTree ID="right_arm_vision_recovery_subtree">
<Sequence name="right_arm_vision_recovery_root">
<Parallel name="right_arm_initial">
<Arm_H name="s17_right_arm_initial" />
</Parallel>
<ClearDoneInstances_H name="clear_done_instances_right_arm_vision" />
</Sequence>
</BehaviorTree>
<!-- 子树：双臂相机同步拍照位 -->
<BehaviorTree ID="dual_arm_cam_subtree">
<Sequence name="cam_sync_root">
<Script code="right_cam_status:=false; left_cam_status:=false" />
<Parallel name="dual_arm_cam_take_photo" success_count="2" failure_count="2">
<!-- 右臂 -->
<Fallback>
<Sequence>
<Arm_H name="s2_right_arm_cam_take_photo" />
<Script code="right_cam_status:=true" />
</Sequence>
<Arm_H name="s15_right_arm_initial" />
<AlwaysSuccess/>
</Fallback>
<!-- 左臂 -->
<Fallback>
<Sequence>
<Arm_H name="s2_left_arm_cam_take_photo" />
<Script code="left_cam_status:=true" />
</Sequence>
<Arm_H name="s15_left_arm_initial" />
<AlwaysSuccess/>
</Fallback>
</Parallel>
<ScriptCondition code="right_cam_status==true || left_cam_status==true" />
</Sequence>
</BehaviorTree>
<!-- 子树：串行手眼视觉识别 -->
<BehaviorTree ID="sequential_hand_vision_subtree">
<Sequence name="hand_vision_root">
<Script code="right_v_ok:=false; left_v_ok:=false" />
<!-- 右臂视觉 -->
<Fallback>
<Sequence>
<ScriptCondition code="right_done==false &amp;&amp; right_cam_ok==true" />
<Fallback>
<Sequence>
<SubTree ID="right_arm_hand_vision_subtree"/>
<Script code="right_v_ok:=true" />
</Sequence>
<AlwaysFailure/>
</Fallback>
</Sequence>
<AlwaysSuccess/>
</Fallback>
<!-- 左臂视觉 -->
<Fallback>
<Sequence>
<ScriptCondition code="left_done==false &amp;&amp; left_cam_ok==true" />
<Fallback>
<Sequence>
<SubTree ID="left_arm_hand_vision_subtree"/>
<Script code="left_v_ok:=true" />
</Sequence>
<AlwaysFailure/>
</Fallback>
</Sequence>
<AlwaysSuccess/>
</Fallback>
</Sequence>
</BehaviorTree>
<!-- 子树：异步双臂抓取 -->
<BehaviorTree ID="dual_arm_grasp_subtree_wrapper">
<Sequence name="grasp_sync_root">
<Parallel name="dual_arm_asynchronous_grasp" success_count="2" failure_count="2">
<!-- 右臂抓取 -->
<Fallback>
<Sequence>
<ScriptCondition code="right_v_ok==true" />
<SubTree ID="right_arm_grasp_subtree"/>
<Script code="right_done:=true" />
</Sequence>
<AlwaysSuccess/>
</Fallback>
<!-- 左臂抓取 -->
<Fallback>
<Sequence>
<ScriptCondition code="left_v_ok==true" />
<SubTree ID="left_arm_grasp_subtree"/>
<Script code="left_done:=true" />
</Sequence>
<AlwaysSuccess/>
</Fallback>
</Parallel>
<ScriptCondition code="right_done==true || left_done==true" />
</Sequence>
</BehaviorTree>
<!-- 子树：双臂放置流程 -->
<BehaviorTree ID="dual_arm_putdown_subtree_wrapper">
<Sequence name="putdown_sync_root">
<Script code="left_p_ok:=false; right_p_ok:=false" />
<Parallel name="dual_arm_putdown" success_count="2" failure_count="2">
<Fallback>
<Sequence>
<ScriptCondition code="left_done==true" />
<SubTree ID="left_arm_putdown_subtree"/>
<Script code="left_p_ok:=true" />
</Sequence>
<AlwaysSuccess/>
</Fallback>
<Fallback>
<Sequence>
<ScriptCondition code="right_done==true" />
<SubTree ID="right_arm_putdown_subtree"/>
<Script code="right_p_ok:=true" />
</Sequence>
<AlwaysSuccess/>
</Fallback>
</Parallel>
<ScriptCondition code="left_p_ok==true || right_p_ok==true" />
</Sequence>
</BehaviorTree>
</root>

10
src/brain/config/bt_vision_grasp_left_arm.params.yaml
View File

@@ -61,7 +61,7 @@
'
- name: s2_left_arm_cam_take_photo
params: 'body_id: 0
params: &arm_cam_take_photo 'body_id: 0
data_type: 100
@@ -113,7 +113,7 @@
torque: 100
mode: 1
mode: 2
'
- name: s4_left_arm_take_box
@@ -211,4 +211,8 @@
- name: s15_arm_pre_grasp
params: *pre_grasp
- name: s16_arm_pre_grasp
params: *pre_grasp
params: *pre_grasp
- name: s15_left_arm_cam_take_photo
params: *arm_cam_take_photo
- name: s16_left_arm_cam_take_photo
params: *arm_cam_take_photo

2
src/brain/config/bt_vision_grasp_left_arm.xml
View File

@@ -143,6 +143,7 @@
<Sequence>
<GripperCmd0_H name="s15_gripper_open" />
<Arm_H name="s15_arm_pre_grasp" />
<Arm_H name="s15_left_arm_cam_take_photo" />
<Arm_H name="s15_left_arm_pre_cam_take_photo" />
<ClearDoneInstances_H name="clear_done_instances_grasp" />
</Sequence>
@@ -156,6 +157,7 @@
<Sequence>
<GripperCmd0_H name="s16_gripper_open" />
<Arm_H name="s16_arm_pre_grasp" />
<Arm_H name="s16_left_arm_cam_take_photo" />
<Arm_H name="s16_left_arm_pre_cam_take_photo" />
<ClearDoneInstances_H name="clear_done_instances_putdown" />
</Sequence>

10
src/brain/config/bt_vision_grasp_right_arm.params.yaml
View File

@@ -61,7 +61,7 @@
'
- name: s2_right_arm_cam_take_photo
params: 'body_id: 1
params: &arm_cam_take_photo 'body_id: 1
data_type: 100
@@ -113,7 +113,7 @@
torque: 100
mode: 1
mode: 2
'
- name: s4_right_arm_take_box
@@ -211,4 +211,8 @@
- name: s15_arm_pre_grasp
params: *pre_grasp
- name: s16_arm_pre_grasp
params: *pre_grasp
params: *pre_grasp
- name: s15_right_arm_cam_take_photo
params: *arm_cam_take_photo
- name: s16_right_arm_cam_take_photo
params: *arm_cam_take_photo

2
src/brain/config/bt_vision_grasp_right_arm.xml
View File

@@ -143,6 +143,7 @@
<Sequence>
<GripperCmd1_H name="s15_gripper_open" />
<Arm_H name="s15_arm_pre_grasp" />
<Arm_H name="s15_right_arm_cam_take_photo" />
<Arm_H name="s15_right_arm_pre_cam_take_photo" />
<ClearDoneInstances_H name="clear_done_instances_grasp" />
</Sequence>
@@ -156,6 +157,7 @@
<Sequence>
<GripperCmd1_H name="s16_gripper_open" />
<Arm_H name="s16_arm_pre_grasp" />
<Arm_H name="s16_right_arm_cam_take_photo" />
<Arm_H name="s16_right_arm_pre_cam_take_photo" />
<ClearDoneInstances_H name="clear_done_instances_putdown" />
</Sequence>

17
src/brain/src/cerebellum_hooks.cpp
View File

@@ -657,7 +657,7 @@ void CerebellumHooks::ConfigureVisionObjectRecognitionHooks(CerebellumNode * nod
if (target_pose.pose.position.z > 0.10f) { //right arm
target_arm = "right_arm";
target_pose.pose.position.x += 0.10f;
target_pose.pose.position.x += 0.12f;
target_pose.pose.position.y = 0.32f;
// target_pose.pose.position.z += 0.03f;
target_pose.pose.orientation.x = -0.7071;
@@ -794,13 +794,14 @@ void CerebellumHooks::ConfigureVisionObjectRecognitionHooks(CerebellumNode * nod
result.grasp_pose.orientation.x(), result.grasp_pose.orientation.y(), result.grasp_pose.orientation.z(), result.grasp_pose.orientation.w());
#endif
if (node->camera_position_ == "right") {
result.grasp_pose.position.setZ(result.grasp_pose.position.z() - 0.015); //补偿抓取的偏差
result.pre_grasp_pose.position.setZ(result.pre_grasp_pose.position.z() - 0.015); //补偿抓取的偏差
} else if (node->camera_position_ == "left") {
result.grasp_pose.position.setZ(result.grasp_pose.position.z() + 0.01); //补偿抓取的偏差
result.pre_grasp_pose.position.setZ(result.pre_grasp_pose.position.z() + 0.01); //补偿抓取的偏差
}
// if (node->camera_position_ == "right") {
// result.grasp_pose.position.setZ(result.grasp_pose.position.z() - 0.015); //补偿抓取的偏差
// result.pre_grasp_pose.position.setZ(result.pre_grasp_pose.position.z() - 0.015); //补偿抓取的偏差
// } else if (node->camera_position_ == "left") {
// result.grasp_pose.position.setZ(result.grasp_pose.position.z() + 0.01); //补偿抓取的偏差
// result.pre_grasp_pose.position.setZ(result.pre_grasp_pose.position.z() + 0.01); //补偿抓取的偏差
// }
//pre grasp
std::vector<double> pre_grasp_joint_angles = current_joint_angles;
if (brain::GraspPoseCalculator::rm_arm_inverse_kinematics(result.pre_grasp_pose.position,