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

optimize bt sch

Browse Source
This commit is contained in:
NuoDaJia02
2026-01-11 16:31:43 +08:00
parent 385527dc2a
commit 0b6f18729c
2 changed files with 132 additions and 131 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -361,4 +361,8 @@
- 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

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

@@ -15,136 +15,16 @@
</Fallback>
<!-- Move both arms to positions where their hand cameras can see potential objects -->
<Script code="right_arm_cam_take_photo:=false; left_arm_cam_take_photo:=false" />
<Sequence name="dual_arm_sync_process">
<Parallel name="dual_arm_cam_take_photo" success_count="2" failure_count="2">
<!-- right arm cam -->
<Fallback>
<Sequence name="right_arm_cam_take_photo_branch">
<Fallback>
<Sequence>
<Arm_H name="s2_right_arm_cam_take_photo" />
</Sequence>
<Sequence>
<SubTree ID="right_arm_vision_recovery_subtree"/>
<AlwaysFailure/>
</Sequence>
</Fallback>
<Script code="right_arm_cam_take_photo:=true" />
</Sequence>
<AlwaysSuccess name="force_wait_left_cam"/>
</Fallback>
<!-- left arm cam -->
<Fallback>
<Sequence name="left_arm_cam_take_photo_branch">
<Fallback>
<Sequence>
<Arm_H name="s2_left_arm_cam_take_photo" />
</Sequence>
<Sequence>
<SubTree ID="left_arm_vision_recovery_subtree"/>
<AlwaysFailure/>
</Sequence>
</Fallback>
<Script code="left_arm_cam_take_photo:=true" />
</Sequence>
<AlwaysSuccess name="force_wait_left_cam"/>
</Fallback>
</Parallel>
<!-- right arm cam or left arm cam -->
<ScriptCondition code="right_arm_cam_take_photo==true || left_arm_cam_take_photo==true" />
</Sequence>
<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 -->
<Sequence name="sequential_hand_vision">
<Script code="right_vision_ok:=false; left_vision_ok:=false" />
<!-- Right Arm Hand Vision -->
<Fallback name="right_arm_hand_vision_exclusive">
<Sequence>
<ScriptCondition code="right_arm_done==false; right_arm_cam_take_photo==true" />
<Fallback name="right_arm_vision_process_with_recovery">
<Sequence>
<SubTree ID="right_arm_hand_vision_subtree"/>
<Script code="right_vision_ok:=true" />
</Sequence>
<Sequence>
<SubTree ID="right_arm_vision_recovery_subtree"/>
<AlwaysFailure/>
</Sequence>
</Fallback>
</Sequence>
<AlwaysSuccess/>
</Fallback>
<!-- Left Arm Hand Vision -->
<Fallback name="left_arm_hand_vision_exclusive">
<Sequence>
<ScriptCondition code="left_arm_done==false; left_arm_cam_take_photo==true" />
<Fallback name="left_arm_vision_process_with_recovery">
<Sequence>
<SubTree ID="left_arm_hand_vision_subtree"/>
<Script code="left_vision_ok:=true" />
</Sequence>
<Sequence>
<SubTree ID="left_arm_vision_recovery_subtree"/>
<AlwaysFailure/>
</Sequence>
</Fallback>
</Sequence>
<AlwaysSuccess/>
</Fallback>
</Sequence>
<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 -->
<!-- Asynchronous execution of both arms -->
<Sequence name="dual_arm_sync_process">
<!-- 1. Parallel execution with a threshold of 2 to ensure both complete -->
<Parallel name="dual_arm_asynchronous_grasp" success_count="2" failure_count="2">
<!-- right arm -->
<Fallback name="right_arm_wrapper">
<Sequence name="right_arm_branch">
<ScriptCondition code="right_vision_ok==true" />
<Fallback name="right_arm_process_with_recovery">
<Sequence name="right_arm_process">
<SubTree ID="right_arm_grasp_subtree"/>
<SubTree ID="right_arm_putdown_subtree"/>
</Sequence>
<Sequence>
<SubTree ID="right_arm_grasp_recovery_subtree"/>
<AlwaysFailure/>
</Sequence>
</Fallback>
<Script code="right_arm_done:=true" />
</Sequence>
<!-- Even if the above fails (vision or grasp failure), return SUCCESS to Parallel to prevent premature FAILURE -->
<AlwaysSuccess name="force_wait_right"/>
</Fallback>
<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}"/>
<!-- left arm -->
<Fallback name="left_arm_wrapper">
<Sequence name="left_arm_branch">
<ScriptCondition code="left_vision_ok==true" />
<Fallback name="left_arm_process_with_recovery">
<Sequence name="left_arm_process">
<SubTree ID="left_arm_grasp_subtree"/>
<SubTree ID="left_arm_putdown_subtree"/>
</Sequence>
<Sequence>
<SubTree ID="left_arm_grasp_recovery_subtree"/>
<AlwaysFailure/>
</Sequence>
</Fallback>
<Script code="left_arm_done:=true" />
</Sequence>
<AlwaysSuccess name="force_wait_left"/>
</Fallback>
</Parallel>
<!-- 2. After parallel execution, perform logical "or" check -->
<!-- As long as one done is true, this step succeeds; otherwise, failure triggers retry -->
<ScriptCondition code="right_arm_done==true || left_arm_done==true" />
</Sequence>
<!-- Phase 4: Dual Arm Putdown -->
<SubTree ID="dual_arm_putdown_subtree_wrapper" right_done="{right_arm_done}" left_done="{left_arm_done}"/>
</Sequence>
<Sequence>
@@ -161,6 +41,8 @@
<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>
@@ -170,7 +52,7 @@
<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="10">
<RetryUntilSuccessful name="retry_top_cam_vision" num_attempts="5">
<VisionObjectRecognition_H name="s2_top_camera_vision_recg" />
</RetryUntilSuccessful>
</Sequence>
@@ -179,7 +61,7 @@
<!-- 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="10">
<RetryUntilSuccessful name="retry_right_hand_cam_vision" num_attempts="5">
<VisionObjectRecognition_H name="s3_right_camera_vision_recg" />
</RetryUntilSuccessful>
</Sequence>
@@ -216,7 +98,7 @@
<!-- 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="10">
<RetryUntilSuccessful name="retry_left_hand_cam_vision" num_attempts="5">
<VisionObjectRecognition_H name="s3_left_camera_vision_recg" />
</RetryUntilSuccessful>
</Sequence>
@@ -277,7 +159,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_initial" />
<!-- <Arm_H name="s15_right_arm_initial" /> -->
<ClearDoneInstances_H name="clear_done_instances_right_grasp" />
</Sequence>
</BehaviorTree>
@@ -286,7 +168,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_initial" />
<!-- <Arm_H name="s15_left_arm_initial" /> -->
<ClearDoneInstances_H name="clear_done_instances_left_grasp" />
</Sequence>
</BehaviorTree>
@@ -319,4 +201,119 @@
</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>
<AlwaysSuccess/>
</Fallback>
<!-- 左臂 -->
<Fallback>
<Sequence>
<Arm_H name="s2_left_arm_cam_take_photo" />
<Script code="left_cam_status:=true" />
</Sequence>
<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>