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

Fix BT params anchors and update configs

Browse Source
This commit is contained in:
NuoDaJia02
2026-02-05 18:07:53 +08:00
parent 0e7b8570fd
commit a02bc399b7
18 changed files with 989 additions and 52 deletions
Download Patch File Download Diff File Expand all files Collapse all files

4
src/brain/config/bt_carry_boxes_sch1.params.yaml
View File

@@ -521,4 +521,8 @@
data_array: [-5, -55, -30, 0, 0, -180, -175, 55, 30, 0, 0, 0]
'
- name: clear_done_instances_retry
params: '{}
'

13
src/brain/config/bt_carry_boxes_sch1.xml
View File

@@ -1,8 +1,13 @@
<!--
功能描述: 机器人搬运箱子流程1（单臂+底盘，3次重试）。
流程: 清理完成标记 -> 机械臂回零 -> 底盘到抓取点 -> 多组拍照位与抓取位动作 -> 抓取抬起
-> 底盘到放置点 -> 腰部下俯与放置并行 -> 松爪 -> 抬臂与腰部上仰并行 -> 回初始位
-> 第二次抓取放置循环 -> 回原点。
功能描述: 搬运箱子流程1（单臂 + 底盘；无视觉；基于预设位抓取/放置；整体3次重试）。
运行概览:
1) 每次重试先清理已完成标记（ClearDoneInstances_H），随后机械臂回零/回初始位。
2) 第一次搬运循环：底盘前往抓取点 -> 依次执行多组“拍照/定位”预设位（s2/s3/s3_top）->
机械臂准备抓取/抓取/抬起 -> 底盘回到中间放置位 -> 腰部下俯与放置动作并行 -> 松爪 ->
抬臂与腰部上仰并行 -> 机械臂回预备位/回初始位。
3) 第二次搬运循环：重复“拍照位 -> 抓取/抬起 -> 回放置位 -> 放置/松爪 -> 抬臂/上仰”流程，
最后底盘回原点、机械臂回初始位。
说明: 全流程仅使用 Arm_H 与 MoveWheel_H/MoveWaist_H 的预设动作序列，无相机识别与条件分支。
-->
<root BTCPP_format="4">
<BehaviorTree ID="MainTree">

4
src/brain/config/bt_carry_boxes_sch2.params.yaml
View File

@@ -537,4 +537,8 @@
data_array: [-5, -55, -30, 0, 0, -180, -175, 55, 30, 0, 0, 0]
'
- name: clear_done_instances_retry
params: '{}
'

13
src/brain/config/bt_carry_boxes_sch2.xml
View File

@@ -1,8 +1,13 @@
<!--
功能描述: 机器人搬运箱子流程2（单臂+底盘+腰部俯仰，3次重试，适配高低位箱体）。
流程: 回零 -> 底盘到抓取点 -> 预抓取位 -> 腰部下俯拍照 -> 多角度拍照识别 -> 抓取
-> 抬起与腰部上仰并行 -> 底盘到放置点 -> 放置/松爪 -> 抬臂 -> 返回拍照位与回零
-> 第二次抓取放置循环。
功能描述: 搬运箱子流程2（单臂 + 底盘 + 腰部俯仰；无视觉；适配高低位；整体3次重试）。
运行概览:
1) 每次重试先清理已完成标记（ClearDoneInstances_H），机械臂回零后底盘移动到抓取点。
2) 第一次抓取：机械臂到预抓取位与拍照位 -> 腰部下俯 -> 依次执行多组“拍照位”预设动作
-> 抓取准备/抓取 -> 抬起与腰部上仰并行 -> 底盘移动到放置位 -> 放置/松爪/抬臂
-> 机械臂回拍照位并回初始位。
3) 第二次抓取：底盘移动到第二抓取点 -> 重复“预抓取位 -> 下俯 -> 拍照位序列 -> 抓取/抬起”
-> 底盘到放置位 -> 放置/松爪/抬臂 -> 回拍照位与初始位 -> 底盘回原点。
说明: 全流程动作由 Arm_H、MoveWheel_H、MoveWaist_H 的预设位驱动，无相机识别与条件分支。
-->
<root BTCPP_format="4">
<BehaviorTree ID="MainTree">

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

@@ -47,14 +47,20 @@
- name: s2_top_camera_vision_recg
params: 'camera_position: top
classes: ["medicine_box"]
'
- name: s3_right_camera_vision_recg
params: 'camera_position: right
classes: ["medicine_box"]
'
- name: s3_left_camera_vision_recg
params: 'camera_position: left
classes: ["medicine_box"]
'
- name: s3_right_arm_vision_pre_grasp
params: &pre_grasp_right "arm_motion_params:\n- arm_id: 1\n motion_type: 101\n target_pose:\n \

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

@@ -1,7 +1,19 @@
<!--
功能描述: 双臂协同视觉抓取流程（不含底盘，含分阶段恢复与完成标记清理）。
流程: 初始化状态变量 -> 头部相机视觉定位（失败进入视觉恢复）-> 双臂同步到手眼拍照位
-> 串行左右手眼视觉识别 -> 双臂异步抓取（各自含抓取恢复）-> 双臂放置 -> 双臂回初始位。
功能描述: 双臂协同视觉抓取流程（不含底盘；顶视+双手眼；分阶段恢复与完成标记清理）。
运行概览:
1) 变量初始化：通过 Script 初始化右/左臂拍照状态、视觉成功标记与完成标记。
2) 顶视视觉阶段：双爪打开、双臂回初始 -> 顶视相机识别（最多5次）；失败则进入视觉恢复子树。
3) 双臂拍照位同步：左右臂并行到手眼拍照位，失败则回初始并允许单臂成功，确保至少一臂可用。
4) 串行手眼识别：右臂手眼视觉（最多5次）-> 左臂手眼视觉（最多5次）；
仅对尚未完成且拍照成功的手臂执行识别。
5) 异步抓取：左右臂并行抓取，各自包含“抓取动作 + 抓取恢复”分支；任一臂成功即可继续。
6) 放置阶段：左右臂并行放置，各自独立执行放置子树；任一臂成功即可继续。
7) 结束归位：双臂并行回初始位。
恢复机制:
- 视觉恢复：双臂回初始并清理视觉完成标记。
- 抓取恢复：左右臂各自执行“松爪/预抓取/拍照位/回初始”，并清理对应完成标记（match=left/right）。
- 手眼视觉恢复：左右臂各自回初始并清理对应视觉标记。
- 总体恢复：双爪打开、双臂回初始并清理整体完成标记。
-->
<root BTCPP_format="4" main_tree_to_execute="MainTree">
<BehaviorTree ID="MainTree">

36
src/brain/config/bt_vision_grasp_dual_arm_demo1.params.yaml
View File

@@ -47,14 +47,20 @@
- name: s2_top_camera_vision_recg
params: 'camera_position: top
classes: ["medicine_box"]
'
- name: s3_right_camera_vision_recg
params: 'camera_position: right
classes: ["medicine_box"]
'
- name: s3_left_camera_vision_recg
params: 'camera_position: left
classes: ["medicine_box"]
'
- name: s3_right_arm_vision_pre_grasp
params: &pre_grasp_right "arm_motion_params:\n- arm_id: 1\n motion_type: 101\n target_pose:\n \
@@ -101,7 +107,7 @@
- name: s5_right_arm_take_box_off
params: "arm_motion_params:\n- arm_id: 1\n motion_type: 0\n target_pose:\n \
\ position:\n x: 0.0\n y: 0.0\n z: 0.0\n orientation:\n \
\ x: 0.0\n y: 0.0\n z: 0.0\n w: 1.0\n target_joints: [96.31, -103.06, 17.69, -53.40, -7.81, 143.19\n blend_radius:\
\ x: 0.0\n y: 0.0\n z: 0.0\n w: 1.0\n target_joints: [96.31, -103.06, 17.69, -53.40, -7.81, 143.19]\n blend_radius:\
\ 0\nvelocity_scaling: 40\n"
- name: s5_left_arm_take_box_off
params: "arm_motion_params:\n- arm_id: 0\n motion_type: 0\n target_pose:\n \
@@ -176,10 +182,6 @@
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
@@ -205,19 +207,19 @@
- 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
params: &origin_pickup_position 'move_distance: 0.5
move_angle: 0.0
'
- name: s6_wheel_move_to_putdown_position
params: 'move_distance: -0.5
params: 'move_distance: 0.0
move_angle: 0.0
'
- name: s6_move_waist_turn_left_90
params: 'move_pitch_degree: 0.0
params: &waist_turn_left_90 'move_pitch_degree: 0.0
move_yaw_degree: 90.0
@@ -233,4 +235,20 @@
- 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
params: *right_arm_cam_take_photo
- name: s15_right_arm_initial_vision
params: *arm_inittial_right
- name: s15_left_arm_initial_vision
params: *arm_inittial_left
- name: s15_right_arm_initial_grasp
params: *arm_inittial_right
- name: s15_left_arm_initial_grasp
params: *arm_inittial_left
- name: s15_right_arm_initial_cam
params: *arm_inittial_right
- name: s15_left_arm_initial_cam
params: *arm_inittial_left
- name: s6_move_waist_turn_right_90
params: *waist_turn_right_90
- name: s10_move_waist_turn_left_90
params: *waist_turn_left_90

52
src/brain/config/bt_vision_grasp_dual_arm_demo1.xml
View File

@@ -1,7 +1,15 @@
<!--
功能描述: 双臂协同视觉抓取演示流程1（全机身协作，含失败恢复与重试）。
流程: 底盘移动到抓取工位 -> 头部相机定位 + 双臂手眼识别 -> 双臂异步抓取
-> 底盘移动到放置工位并腰部旋转 -> 双臂放置 -> 腰部复位 -> 底盘回抓取位 -> 双臂回初始位。
功能描述: 双臂协同视觉抓取演示流程1（全机身协作；顶视+双手眼；含恢复与重试）。
运行概览:
1) 起始移动：底盘移动到抓取工位。
2) 顶视视觉阶段：双爪打开、双臂回初始 -> 顶视相机识别（最多5次）；失败则进入视觉恢复。
3) 双臂拍照位同步：左右臂并行到手眼拍照位，允许单臂成功。
4) 串行手眼识别：右臂手眼 -> 左臂手眼（各最多5次），仅对可用且未完成的手臂执行。
5) 异步抓取：左右臂并行抓取，各自含抓取恢复；任一臂成功即可继续。
6) 放置移动：底盘到放置工位 + 腰部右转并行。
7) 放置阶段：左右臂并行放置，各自独立完成。
8) 复位：腰部左转复位；底盘回抓取位；双臂回初始位。
恢复机制: 视觉/抓取/手眼各自有恢复子树，失败时触发重试；外层总流程最多重试3次。
-->
<root BTCPP_format="4" main_tree_to_execute="MainTree">
<BehaviorTree ID="MainTree">
@@ -33,17 +41,17 @@
<Parallel name="parallel_action_2">
<MoveWheel_H name="s6_wheel_move_to_putdown_position" />
<MoveWaist_H name="s6_move_waist_turn_left_90" />
<MoveWaist_H name="s6_move_waist_turn_right_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" />
<MoveWaist_H name="s10_move_waist_turn_left_90" />
<!-- Phase 5: Dual Arm Initialization -->
<Parallel name="dual_arm_init_action">
<MoveWheel_H name="s10_wheel_move_to_origin_pickup_position" />
<!-- <MoveWheel_H name="s10_wheel_move_to_origin_pickup_position" /> -->
<DualArm_H name="s11_right_arm_initial" />
<DualArm_H name="s11_left_arm_initial" />
</Parallel>
@@ -83,11 +91,15 @@
<!-- Right Arm Specific Subtrees -->
<BehaviorTree ID="right_DualArm_Hand_vision_subtree">
<Sequence name="right_hand_vision_root">
<Fallback name="right_hand_vision_fallback">
<RetryUntilSuccessful name="retry_right_hand_cam_vision" num_attempts="5">
<VisionObjectRecognition_H name="s3_right_camera_vision_recg" />
</RetryUntilSuccessful>
</Sequence>
<Sequence>
<DualArm_H name="s15_right_arm_initial_vision" />
<AlwaysFailure />
</Sequence>
</Fallback>
</BehaviorTree>
<BehaviorTree ID="right_arm_grasp_subtree">
@@ -120,11 +132,15 @@
<!-- Left Arm Specific Subtrees -->
<BehaviorTree ID="left_DualArm_Hand_vision_subtree">
<Sequence name="left_hand_vision_root">
<Fallback name="left_hand_vision_fallback">
<RetryUntilSuccessful name="retry_left_hand_cam_vision" num_attempts="5">
<VisionObjectRecognition_H name="s3_left_camera_vision_recg" />
</RetryUntilSuccessful>
</Sequence>
<Sequence>
<DualArm_H name="s15_left_arm_initial_vision" />
<AlwaysFailure />
</Sequence>
</Fallback>
</BehaviorTree>
<BehaviorTree ID="left_arm_grasp_subtree">
@@ -183,8 +199,8 @@
<GripperCmd1_H name="s15_right_hand_gripper_open" />
<DualArm_H name="s15_right_arm_pre_grasp" />
<DualArm_H name="s15_right_arm_cam_take_photo" />
<DualArm_H name="s15_right_arm_initial" />
<ClearDoneInstances_H name="clear_done_instances_right_grasp" />
<DualArm_H name="s15_right_arm_initial_grasp" />
<ClearDoneInstances_H name="clear_done_instances_right_grasp" match="right" />
</Sequence>
</BehaviorTree>
@@ -193,8 +209,8 @@
<GripperCmd0_H name="s15_left_hand_gripper_open" />
<DualArm_H name="s15_left_arm_pre_grasp" />
<DualArm_H name="s15_left_arm_cam_take_photo" />
<DualArm_H name="s15_left_arm_initial" />
<ClearDoneInstances_H name="clear_done_instances_left_grasp" />
<DualArm_H name="s15_left_arm_initial_grasp" />
<ClearDoneInstances_H name="clear_done_instances_left_grasp" match="left" />
</Sequence>
</BehaviorTree>
@@ -213,7 +229,7 @@
<Parallel name="left_arm_initial">
<DualArm_H name="s17_left_arm_initial" />
</Parallel>
<ClearDoneInstances_H name="clear_done_instances_left_arm_vision" />
<ClearDoneInstances_H name="clear_done_instances_left_arm_vision" match="left" />
</Sequence>
</BehaviorTree>
@@ -222,7 +238,7 @@
<Parallel name="right_arm_initial">
<DualArm_H name="s17_right_arm_initial" />
</Parallel>
<ClearDoneInstances_H name="clear_done_instances_right_arm_vision" />
<ClearDoneInstances_H name="clear_done_instances_right_arm_vision" match="right" />
</Sequence>
</BehaviorTree>
@@ -238,7 +254,7 @@
<DualArm_H name="s2_right_arm_cam_take_photo" />
<Script code="right_cam_status:=true" />
</Sequence>
<DualArm_H name="s15_right_arm_initial" />
<DualArm_H name="s15_right_arm_initial_cam" />
<AlwaysSuccess/>
</Fallback>
<!-- 左臂 -->
@@ -247,7 +263,7 @@
<DualArm_H name="s2_left_arm_cam_take_photo" />
<Script code="left_cam_status:=true" />
</Sequence>
<DualArm_H name="s15_left_arm_initial" />
<DualArm_H name="s15_left_arm_initial_cam" />
<AlwaysSuccess/>
</Fallback>
</Parallel>

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

@@ -51,6 +51,8 @@
- name: s2_top_camera_vision_recg
params: 'camera_position: top
classes: ["medicine_box"]
'
- name: s2_left_arm_cam_take_photo
params: &arm_cam_take_photo "arm_motion_params:\n- arm_id: 0\n motion_type: 100\n target_pose:\n \
@@ -60,6 +62,8 @@
- name: s3_left_camera_vision_recg
params: 'camera_position: left
classes: ["medicine_box"]
'
- name: s3_left_arm_vision_pre_grasp
params: &pre_grasp "arm_motion_params:\n- arm_id: 0\n motion_type: 101\n target_pose:\n \
@@ -169,4 +173,20 @@
- name: s15_left_arm_cam_take_photo
params: *arm_cam_take_photo
- name: s16_left_arm_cam_take_photo
params: *arm_cam_take_photo
params: *arm_cam_take_photo
- name: clear_done_instances_overall
params: '{}
'
- name: clear_done_instances_vision
params: '{}
'
- name: clear_done_instances_grasp
params: '{}
'
- name: clear_done_instances_putdown
params: '{}
'

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

@@ -1,6 +1,15 @@
<!--
功能描述: 左臂独立视觉抓取流程（视觉/抓取/放置均带恢复与重试）。
流程: 左臂到手眼拍照位 -> 相机识别目标 -> 预抓取/抓取/抬起 -> 送到放置位并松爪 -> 回到初始位。
功能描述: 左臂视觉抓取流程（顶视+手眼识别；抓取/放置含恢复；外层3次重试）。
运行概览:
1) 视觉阶段（vision_subtree）：双臂回初始并打开左爪；进行顶视相机识别（最多10次）；
左臂到手眼拍照位后进行左手眼识别（最多10次）。
2) 抓取阶段（grasp_subtree）：左臂预抓取 -> 左臂抓取 -> 关闭左爪 -> 左臂抬起；
随后左右臂并行保持抓箱位（parallel_action_grasp_box）。
3) 放置阶段（putdown_subtree）：左右臂并行抓箱位/放箱位 -> 左爪松开 -> 左臂回抓箱位；
双臂并行回初始位。
4) 恢复机制：视觉/抓取/放置各自失败时进入对应恢复子树（复位/重新拍照/清理完成标记），
并通过 AlwaysFailure 触发阶段重试；外层总流程最多重试3次。
说明: 底盘与腰部动作在此版本中被注释，流程主要为视觉+双臂姿态与左爪控制。
-->
<root BTCPP_format="4" main_tree_to_execute="MainTree">
<BehaviorTree ID="MainTree">

196
src/brain/config/bt_vision_grasp_left_arm_demo1.params.yaml Normal file
View File

@@ -0,0 +1,196 @@
- name: root
params: '{}
'
- name: retry_vision_hand
params: '{}
'
- name: s1_move_waist_turn_right_90
params: &waist_turn_right_90 'move_pitch_degree: 0.0
move_yaw_degree: -90.0
'
- name: s1_gripper_open
params: &gripper_open 'loc: 0
speed: 150
torque: 20
mode: 2
'
- name: s1_left_arm_initial
params: &left_arm_initial "arm_motion_params:\n- arm_id: 0\n motion_type: 0\n target_pose:\n \
\ position:\n x: 0.0\n y: 0.0\n z: 0.0\n orientation:\n \
\ x: 0.0\n y: 0.0\n z: 0.0\n w: 1.0\n target_joints: [100, 5, 112, 12, 62, 5]\n blend_radius:\
\ 0\nvelocity_scaling: 40\n"
- name: s1_right_arm_initial
params: &right_arm_initial "arm_motion_params:\n- arm_id: 1\n motion_type: 0\n target_pose:\n \
\ position:\n x: 0.0\n y: 0.0\n z: 0.0\n orientation:\n \
\ x: 0.0\n y: 0.0\n z: 0.0\n w: 1.0\n target_joints: [80, -5, -112, -12, -62, 175]\n blend_radius:\
\ 0\nvelocity_scaling: 40\n"
- name: s1_wheel_move_to_origin_pickup_position
params: 'move_distance: 0.5
move_angle: 0.0
'
- name: s2_left_arm_pre_cam_take_photo
params: "arm_motion_params:\n- arm_id: 0\n motion_type: 0\n target_pose:\n \
\ position:\n x: 0.0\n y: 0.0\n z: 0.0\n orientation:\n \
\ x: 0.0\n y: 0.0\n z: 0.0\n w: 1.0\n target_joints: [165.80, 34.07, 122.53, 76.89, -84.40, 67.20]\n blend_radius:\
\ 0\nvelocity_scaling: 40\n"
- name: s2_right_arm_pre_cam_take_photo
params: "arm_motion_params:\n- arm_id: 1\n motion_type: 0\n target_pose:\n \
\ position:\n x: 0.0\n y: 0.0\n z: 0.0\n orientation:\n \
\ x: 0.0\n y: 0.0\n z: 0.0\n w: 1.0\n target_joints: [14.29, -34.07, -122.53, -76.89, 84.40, 112.89]\n blend_radius:\
\ 0\nvelocity_scaling: 40\n"
- name: s2_top_camera_vision_recg
params: 'camera_position: top
classes: ["medicine_box"]
'
- name: s2_left_arm_cam_take_photo
params: &arm_cam_take_photo "arm_motion_params:\n- arm_id: 0\n motion_type: 100\n target_pose:\n \
\ position:\n x: 0.0\n y: 0.0\n z: 0.0\n orientation:\n \
\ x: 0.0\n y: 0.0\n z: 0.0\n w: 1.0\n target_joints: [360, 360, 360, 360, 360, 360]\n blend_radius:\
\ 0\nvelocity_scaling: 40\n"
- name: s3_left_camera_vision_recg
params: 'camera_position: left
classes: ["medicine_box"]
'
- name: s3_left_arm_vision_pre_grasp
params: &pre_grasp "arm_motion_params:\n- arm_id: 0\n motion_type: 101\n target_pose:\n \
\ position:\n x: 0.0\n y: 0.0\n z: 0.0\n orientation:\n \
\ x: 0.0\n y: 0.0\n z: 0.0\n w: 1.0\n target_joints: [360, 360, 360, 360, 360, 360]\n blend_radius:\
\ 0\nvelocity_scaling: 40\n"
- name: s4_left_arm_vision_grasp
params: "arm_motion_params:\n- arm_id: 0\n motion_type: 102\n target_pose:\n \
\ position:\n x: 0.0\n y: 0.0\n z: 0.0\n orientation:\n \
\ x: 0.0\n y: 0.0\n z: 0.0\n w: 1.0\n target_joints: [360, 360, 360, 360, 360, 360]\n blend_radius:\
\ 0\nvelocity_scaling: 40\n"
- name: s4_gripper_close
params: 'loc: 200
speed: 150
torque: 100
mode: 2
'
- name: s4_left_arm_take_box
params: "arm_motion_params:\n- arm_id: 0\n motion_type: 110\n target_pose:\n \
\ position:\n x: 0.0\n y: 0.0\n z: 0.0\n orientation:\n \
\ x: 0.0\n y: 0.0\n z: 0.0\n w: 1.0\n target_joints: [360, 360, 360, 360, 360, 360]\n blend_radius:\
\ 0\nvelocity_scaling: 40\n"
- name: s5_left_arm_grasp_box
params: "arm_motion_params:\n- arm_id: 0\n motion_type: 0\n target_pose:\n \
\ position:\n x: 0.0\n y: 0.0\n z: 0.0\n orientation:\n \
\ x: 0.0\n y: 0.0\n z: 0.0\n w: 1.0\n target_joints: [84, 88, 45, 22, 16, 68]\n blend_radius:\
\ 0\nvelocity_scaling: 40\n"
- name: s5_right_arm_grasp_box
params: "arm_motion_params:\n- arm_id: 1\n motion_type: 0\n target_pose:\n \
\ position:\n x: 0.0\n y: 0.0\n z: 0.0\n orientation:\n \
\ x: 0.0\n y: 0.0\n z: 0.0\n w: 1.0\n target_joints: [95, -25, -90, 0, -68, 90]\n blend_radius:\
\ 0\nvelocity_scaling: 40\n"
- name: s6_move_waist_turn_left_90
params: &waist_turn_left_90 'move_pitch_degree: 0.0
move_yaw_degree: 90.0
'
- name: s6_wheel_move_to_putdown_position
params: 'move_distance: 0.0
move_angle: 0.0
'
- name: s7_left_arm_grasp_box
params: &grasp_box_s7_s9 "arm_motion_params:\n- arm_id: 0\n motion_type: 0\n target_pose:\n \
\ position:\n x: 0.0\n y: 0.0\n z: 0.0\n orientation:\n \
\ x: 0.0\n y: 0.0\n z: 0.0\n w: 1.0\n target_joints: [85, 88, -14, 22, 16, 68]\n blend_radius:\
\ 0\nvelocity_scaling: 40\n"
- name: s7_right_arm_grasp_box
params: "arm_motion_params:\n- arm_id: 1\n motion_type: 0\n target_pose:\n \
\ position:\n x: 0.0\n y: 0.0\n z: 0.0\n orientation:\n \
\ x: 0.0\n y: 0.0\n z: 0.0\n w: 1.0\n target_joints: [95, -25, -90, 0, -68, 90]\n blend_radius:\
\ 0\nvelocity_scaling: 40\n"
- name: s7_left_arm_put_down_box
params: "arm_motion_params:\n- arm_id: 0\n motion_type: 0\n target_pose:\n \
\ position:\n x: 0.0\n y: 0.0\n z: 0.0\n orientation:\n \
\ x: 0.0\n y: 0.0\n z: 0.0\n w: 1.0\n target_joints: [95, 88, -14, -22, 16, 112]\n blend_radius:\
\ 0\nvelocity_scaling: 40\n"
- name: s7_right_arm_put_down_box
params: "arm_motion_params:\n- arm_id: 1\n motion_type: 0\n target_pose:\n \
\ position:\n x: 0.0\n y: 0.0\n z: 0.0\n orientation:\n \
\ x: 0.0\n y: 0.0\n z: 0.0\n w: 1.0\n target_joints: [95, -25, -90, 0, -68, 90]\n blend_radius:\
\ 0\nvelocity_scaling: 40\n"
- name: s8_gripper_open
params: *gripper_open
- name: s9_left_arm_grasp_box
params: *grasp_box_s7_s9
- name: s10_move_waist_turn_right_90
params: *waist_turn_right_90
- name: s11_left_arm_initial
params: *left_arm_initial
- name: s11_right_arm_initial
params: *right_arm_initial
- name: s12_gripper_open
params: *gripper_open
- name: s13_left_arm_initial
params: *left_arm_initial
- name: s13_right_arm_initial
params: *right_arm_initial
- name: s14_left_arm_initial
params: *left_arm_initial
- name: s14_right_arm_initial
params: *right_arm_initial
- name: s15_left_arm_initial
params: *left_arm_initial
- name: s15_right_arm_initial
params: *right_arm_initial
- name: s16_left_arm_initial
params: *left_arm_initial
- name: s16_right_arm_initial
params: *right_arm_initial
- name: s14_gripper_open
params: *gripper_open
- name: s15_gripper_open
params: *gripper_open
- name: s16_gripper_open
params: *gripper_open
- name: s15_arm_pre_grasp
params: *pre_grasp
- name: s16_arm_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
- name: s6_move_waist_turn_right_90
params: *waist_turn_right_90
- name: s10_move_waist_turn_left_90
params: *waist_turn_left_90
- name: clear_done_instances_overall
params: '{}
'
- name: clear_done_instances_vision
params: '{}
'
- name: clear_done_instances_grasp
params: '{}
'
- name: clear_done_instances_putdown
params: '{}
'

196
src/brain/config/bt_vision_grasp_left_arm_demo1.xml Normal file
View File

@@ -0,0 +1,196 @@
<!--
功能描述: 左臂视觉抓取演示流程1（顶视 + 左手眼识别；含底盘与腰部转向；外层3次重试）。
运行概览:
1) 视觉阶段：双臂回初始并打开左爪（GripperCmd0）；底盘移动到取物位；顶视相机识别（最多10次）；
左臂到手眼拍照位后进行左手眼识别（最多10次）。
2) 抓取阶段：左臂预抓取 -> 左臂抓取 -> 关闭左爪 -> 左臂抬起 -> 左右臂并行保持抓箱位。
3) 放置阶段：底盘移动到放置位并腰部右转；左右臂并行抓箱位/放箱位 -> 左爪松开 -> 左臂回抓箱位；
腰部左转复位，同时双臂回初始位。
4) 恢复机制：视觉/抓取/放置各自失败时进入对应恢复子树（复位/重新拍照/清理完成标记），
并通过 AlwaysFailure 触发阶段重试。
-->
<root BTCPP_format="4" main_tree_to_execute="MainTree">
<BehaviorTree ID="MainTree">
<Sequence name="root">
<RetryUntilSuccessful name="retry_vision_grasp" num_attempts="3">
<Fallback name="fallback_main_action">
<Sequence name="complete_pick_and_place">
<!-- Vision with recovery -->
<Fallback name="vision_with_recovery">
<SubTree ID="vision_subtree"/>
<Sequence>
<SubTree ID="vision_recovery_subtree"/>
<AlwaysFailure/>
</Sequence>
</Fallback>
<!-- Grasp with recovery -->
<Fallback name="grasp_with_recovery">
<SubTree ID="grasp_subtree"/>
<Sequence>
<SubTree ID="grasp_recovery_subtree"/>
<AlwaysFailure/>
</Sequence>
</Fallback>
<!-- Putdown with recovery -->
<Fallback name="putdown_with_recovery">
<SubTree ID="putdown_subtree"/>
<Sequence>
<SubTree ID="putdown_recovery_subtree"/>
<AlwaysFailure/>
</Sequence>
</Fallback>
</Sequence>
<!-- Overall recovery if the entire sequence fails -->
<!-- <Sequence>
<SubTree ID="recovery_subtree"/>
<AlwaysFailure/>
</Sequence> -->
</Fallback>
</RetryUntilSuccessful>
</Sequence>
</BehaviorTree>
<BehaviorTree ID="vision_subtree">
<Sequence name="vision_root">
<RetryUntilSuccessful name="retry_vision" num_attempts="1">
<Sequence>
<Parallel name="parallel_action_1">
<DualArm_H name="s1_left_arm_initial" />
<DualArm_H name="s1_right_arm_initial" />
<GripperCmd0_H name="s1_gripper_open" />
</Parallel>
<Parallel name="parallel_action_2">
<MoveWheel_H name="s1_wheel_move_to_origin_pickup_position" />
<!-- <GripperCmd0_H name="s1_gripper_open" /> -->
<!-- <DualArm_H name="s2_left_arm_pre_cam_take_photo" />
<DualArm_H name="s2_right_arm_pre_cam_take_photo" /> -->
</Parallel>
<SubTree ID="vision_top_cam_subtree"/>
<DualArm_H name="s2_left_arm_cam_take_photo" />
<SubTree ID="vision_hand_cam_subtree"/>
</Sequence>
</RetryUntilSuccessful>
</Sequence>
</BehaviorTree>
<BehaviorTree ID="vision_top_cam_subtree">
<Sequence name="vision_top_cam_root">
<RetryUntilSuccessful name="retry_top_cam_vision_recg_1" num_attempts="10">
<VisionObjectRecognition_H name="s2_top_camera_vision_recg" />
</RetryUntilSuccessful>
</Sequence>
</BehaviorTree>
<BehaviorTree ID="vision_hand_cam_subtree">
<Sequence name="vision_hand_cam_root">
<RetryUntilSuccessful name="retry_hand_cam_vision_recg_1" num_attempts="10">
<VisionObjectRecognition_H name="s3_left_camera_vision_recg" />
</RetryUntilSuccessful>
</Sequence>
</BehaviorTree>
<BehaviorTree ID="grasp_subtree">
<Sequence name="grasp_root">
<RetryUntilSuccessful name="retry_grasp" num_attempts="1">
<Sequence>
<DualArm_H name="s3_left_arm_vision_pre_grasp" />
<DualArm_H name="s4_left_arm_vision_grasp" />
<GripperCmd0_H name="s4_gripper_close" />
<DualArm_H name="s4_left_arm_take_box" />
<Parallel name="parallel_action_grasp_box">
<DualArm_H name="s5_left_arm_grasp_box" />
<DualArm_H name="s5_right_arm_grasp_box" />
</Parallel>
</Sequence>
</RetryUntilSuccessful>
</Sequence>
</BehaviorTree>
<BehaviorTree ID="putdown_subtree">
<Sequence name="putdown_root">
<RetryUntilSuccessful name="retry_putdown" num_attempts="1">
<Sequence>
<Parallel name="parallel_action_2">
<MoveWheel_H name="s6_wheel_move_to_putdown_position" />
<MoveWaist_H name="s6_move_waist_turn_right_90" />
</Parallel>
<Parallel name="parallel_action_s7_grasp_box">
<DualArm_H name="s7_left_arm_grasp_box" />
<DualArm_H name="s7_right_arm_grasp_box" />
</Parallel>
<Parallel name="parallel_action_s7_put_down_box">
<DualArm_H name="s7_left_arm_put_down_box" />
<DualArm_H name="s7_right_arm_put_down_box" />
</Parallel>
<GripperCmd0_H name="s8_gripper_open" />
<DualArm_H name="s9_left_arm_grasp_box" />
<MoveWaist_H name="s10_move_waist_turn_left_90" />
<Parallel name="parallel_action_3">
<DualArm_H name="s11_left_arm_initial" />
<DualArm_H name="s11_right_arm_initial" />
</Parallel>
</Sequence>
</RetryUntilSuccessful>
</Sequence>
</BehaviorTree>
<BehaviorTree ID="recovery_subtree">
<Sequence name="recovery_root">
<RetryUntilSuccessful name="retry_recovery" num_attempts="1">
<Sequence>
<GripperCmd0_H name="s12_gripper_open" />
<DualArm_H name="s13_left_arm_initial" />
<DualArm_H name="s13_right_arm_initial" />
<ClearDoneInstances_H name="clear_done_instances_overall" />
</Sequence>
</RetryUntilSuccessful>
</Sequence>
</BehaviorTree>
<BehaviorTree ID="vision_recovery_subtree">
<Sequence name="vision_recovery_root">
<RetryUntilSuccessful name="retry_vision_recovery" num_attempts="1">
<Sequence>
<!-- <GripperCmd0_H name="s14_gripper_open" /> -->
<DualArm_H name="s14_left_arm_initial" />
<DualArm_H name="s14_right_arm_initial" />
<ClearDoneInstances_H name="clear_done_instances_vision" />
</Sequence>
</RetryUntilSuccessful>
</Sequence>
</BehaviorTree>
<BehaviorTree ID="grasp_recovery_subtree">
<Sequence name="grasp_recovery_root">
<RetryUntilSuccessful name="retry_grasp_recovery" num_attempts="1">
<Sequence>
<GripperCmd0_H name="s15_gripper_open" />
<DualArm_H name="s15_arm_pre_grasp" />
<DualArm_H name="s15_left_arm_cam_take_photo" />
<DualArm_H name="s15_left_arm_initial" />
<DualArm_H name="s15_right_arm_initial" />
<ClearDoneInstances_H name="clear_done_instances_grasp" />
</Sequence>
</RetryUntilSuccessful>
</Sequence>
</BehaviorTree>
<BehaviorTree ID="putdown_recovery_subtree">
<Sequence name="putdown_recovery_root">
<RetryUntilSuccessful name="retry_putdown_recovery" num_attempts="1">
<Sequence>
<GripperCmd0_H name="s16_gripper_open" />
<DualArm_H name="s16_arm_pre_grasp" />
<DualArm_H name="s16_left_arm_cam_take_photo" />
<DualArm_H name="s16_left_arm_initial" />
<DualArm_H name="s16_right_arm_initial" />
<ClearDoneInstances_H name="clear_done_instances_putdown" />
</Sequence>
</RetryUntilSuccessful>
</Sequence>
</BehaviorTree>
</root>

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

@@ -51,6 +51,8 @@
- name: s2_top_camera_vision_recg
params: 'camera_position: top
classes: ["medicine_box"]
'
- name: s2_right_arm_cam_take_photo
params: &arm_cam_take_photo "arm_motion_params:\n- arm_id: 1\n motion_type: 100\n target_pose:\n \
@@ -60,6 +62,8 @@
- name: s3_right_camera_vision_recg
params: 'camera_position: right
classes: ["medicine_box"]
'
- name: s3_right_arm_vision_pre_grasp
params: &pre_grasp "arm_motion_params:\n- arm_id: 1\n motion_type: 101\n target_pose:\n \
@@ -169,4 +173,20 @@
- name: s15_right_arm_cam_take_photo
params: *arm_cam_take_photo
- name: s16_right_arm_cam_take_photo
params: *arm_cam_take_photo
params: *arm_cam_take_photo
- name: clear_done_instances_overall
params: '{}
'
- name: clear_done_instances_vision
params: '{}
'
- name: clear_done_instances_grasp
params: '{}
'
- name: clear_done_instances_putdown
params: '{}
'

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

@@ -1,6 +1,15 @@
<!--
功能描述: 右臂独立视觉抓取流程（视觉/抓取/放置均带恢复与重试）。
流程: 右臂到手眼拍照位 -> 相机识别目标 -> 预抓取/抓取/抬起 -> 送到放置位并松爪 -> 回到初始位。
功能描述: 右臂视觉抓取流程（顶视+手眼识别；抓取/放置含恢复；外层3次重试）。
运行概览:
1) 视觉阶段（vision_subtree）：双臂回初始并打开右爪；进行顶视相机识别（最多10次）；
右臂到手眼拍照位后进行右手眼识别（最多10次）。
2) 抓取阶段（grasp_subtree）：右臂预抓取 -> 右臂抓取 -> 关闭右爪 -> 右臂抬起；
随后左右臂并行保持抓箱位（parallel_action_grasp_box）。
3) 放置阶段（putdown_subtree）：左右臂并行抓箱位/放箱位 -> 右爪松开 -> 右臂回抓箱位；
双臂并行回初始位。
4) 恢复机制：视觉/抓取/放置各自失败时进入对应恢复子树（复位/重新拍照/清理完成标记），
并通过 AlwaysFailure 触发阶段重试；外层总流程最多重试3次。
说明: 底盘与腰部动作在此版本中被注释，流程主要为视觉+双臂姿态与右爪控制。
-->
<root BTCPP_format="4" main_tree_to_execute="MainTree">
<BehaviorTree ID="MainTree">

196
src/brain/config/bt_vision_grasp_right_arm_demo1.params.yaml Normal file
View File

@@ -0,0 +1,196 @@
- name: root
params: '{}
'
- name: retry_vision_hand
params: '{}
'
- name: s1_move_waist_turn_right_90
params: &waist_turn_right_90 'move_pitch_degree: 0.0
move_yaw_degree: -90.0
'
- name: s1_gripper_open
params: &gripper_open 'loc: 0
speed: 150
torque: 20
mode: 2
'
- name: s1_left_arm_initial
params: &left_arm_initial "arm_motion_params:\n- arm_id: 0\n motion_type: 0\n target_pose:\n \
\ position:\n x: 0.0\n y: 0.0\n z: 0.0\n orientation:\n \
\ x: 0.0\n y: 0.0\n z: 0.0\n w: 1.0\n target_joints: [100, 5, 112, 12, 62, 5]\n blend_radius:\
\ 0\nvelocity_scaling: 40\n"
- name: s1_right_arm_initial
params: &right_arm_initial "arm_motion_params:\n- arm_id: 1\n motion_type: 0\n target_pose:\n \
\ position:\n x: 0.0\n y: 0.0\n z: 0.0\n orientation:\n \
\ x: 0.0\n y: 0.0\n z: 0.0\n w: 1.0\n target_joints: [80, -5, -112, -12, -62, 175]\n blend_radius:\
\ 0\nvelocity_scaling: 40\n"
- name: s1_wheel_move_to_origin_pickup_position
params: 'move_distance: 0.5
move_angle: 0.0
'
- name: s2_left_arm_pre_cam_take_photo
params: "arm_motion_params:\n- arm_id: 0\n motion_type: 0\n target_pose:\n \
\ position:\n x: 0.0\n y: 0.0\n z: 0.0\n orientation:\n \
\ x: 0.0\n y: 0.0\n z: 0.0\n w: 1.0\n target_joints: [165.80, 34.07, 122.53, 76.89, -84.40, 67.20]\n blend_radius:\
\ 0\nvelocity_scaling: 40\n"
- name: s2_right_arm_pre_cam_take_photo
params: "arm_motion_params:\n- arm_id: 1\n motion_type: 0\n target_pose:\n \
\ position:\n x: 0.0\n y: 0.0\n z: 0.0\n orientation:\n \
\ x: 0.0\n y: 0.0\n z: 0.0\n w: 1.0\n target_joints: [14.29, -34.07, -122.53, -76.89, 84.40, 112.89]\n blend_radius:\
\ 0\nvelocity_scaling: 40\n"
- name: s2_top_camera_vision_recg
params: 'camera_position: top
classes: ["medicine_box"]
'
- name: s2_right_arm_cam_take_photo
params: &arm_cam_take_photo "arm_motion_params:\n- arm_id: 1\n motion_type: 100\n target_pose:\n \
\ position:\n x: 0.0\n y: 0.0\n z: 0.0\n orientation:\n \
\ x: 0.0\n y: 0.0\n z: 0.0\n w: 1.0\n target_joints: [360, 360, 360, 360, 360, 360]\n blend_radius:\
\ 0\nvelocity_scaling: 40\n"
- name: s3_right_camera_vision_recg
params: 'camera_position: right
classes: ["medicine_box"]
'
- name: s3_right_arm_vision_pre_grasp
params: &pre_grasp "arm_motion_params:\n- arm_id: 1\n motion_type: 101\n target_pose:\n \
\ position:\n x: 0.0\n y: 0.0\n z: 0.0\n orientation:\n \
\ x: 0.0\n y: 0.0\n z: 0.0\n w: 1.0\n target_joints: [360, 360, 360, 360, 360, 360]\n blend_radius:\
\ 0\nvelocity_scaling: 40\n"
- name: s4_right_arm_vision_grasp
params: "arm_motion_params:\n- arm_id: 1\n motion_type: 102\n target_pose:\n \
\ position:\n x: 0.0\n y: 0.0\n z: 0.0\n orientation:\n \
\ x: 0.0\n y: 0.0\n z: 0.0\n w: 1.0\n target_joints: [360, 360, 360, 360, 360, 360]\n blend_radius:\
\ 0\nvelocity_scaling: 40\n"
- name: s4_gripper_close
params: 'loc: 200
speed: 150
torque: 100
mode: 2
'
- name: s4_right_arm_take_box
params: "arm_motion_params:\n- arm_id: 1\n motion_type: 110\n target_pose:\n \
\ position:\n x: 0.0\n y: 0.0\n z: 0.0\n orientation:\n \
\ x: 0.0\n y: 0.0\n z: 0.0\n w: 1.0\n target_joints: [360, 360, 360, 360, 360, 360]\n blend_radius:\
\ 0\nvelocity_scaling: 40\n"
- name: s5_left_arm_grasp_box
params: "arm_motion_params:\n- arm_id: 0\n motion_type: 0\n target_pose:\n \
\ position:\n x: 0.0\n y: 0.0\n z: 0.0\n orientation:\n \
\ x: 0.0\n y: 0.0\n z: 0.0\n w: 1.0\n target_joints: [85, 25, 90, 0, 68, 90]\n blend_radius:\
\ 0\nvelocity_scaling: 40\n"
- name: s5_right_arm_grasp_box
params: "arm_motion_params:\n- arm_id: 1\n motion_type: 0\n target_pose:\n \
\ position:\n x: 0.0\n y: 0.0\n z: 0.0\n orientation:\n \
\ x: 0.0\n y: 0.0\n z: 0.0\n w: 1.0\n target_joints: [96.30, -88.95, -45, -22.88, -16.30, 112.10]\n blend_radius:\
\ 0\nvelocity_scaling: 40\n"
- name: s6_move_waist_turn_left_90
params: &waist_turn_left_90 'move_pitch_degree: 0.0
move_yaw_degree: 90.0
'
- name: s6_wheel_move_to_putdown_position
params: 'move_distance: 0.0
move_angle: 0.0
'
- name: s7_left_arm_grasp_box
params: "arm_motion_params:\n- arm_id: 0\n motion_type: 0\n target_pose:\n \
\ position:\n x: 0.0\n y: 0.0\n z: 0.0\n orientation:\n \
\ x: 0.0\n y: 0.0\n z: 0.0\n w: 1.0\n target_joints: [85, 25, 90, 0, 68, 90]\n blend_radius:\
\ 0\nvelocity_scaling: 40\n"
- name: s7_right_arm_grasp_box
params: &grasp_box_s7_s9 "arm_motion_params:\n- arm_id: 1\n motion_type: 0\n target_pose:\n \
\ position:\n x: 0.0\n y: 0.0\n z: 0.0\n orientation:\n \
\ x: 0.0\n y: 0.0\n z: 0.0\n w: 1.0\n target_joints: [96.30, -88.95, 13.98, -22.88, -16.30, 112.10]\n blend_radius:\
\ 0\nvelocity_scaling: 40\n"
- name: s7_left_arm_put_down_box
params: "arm_motion_params:\n- arm_id: 0\n motion_type: 0\n target_pose:\n \
\ position:\n x: 0.0\n y: 0.0\n z: 0.0\n orientation:\n \
\ x: 0.0\n y: 0.0\n z: 0.0\n w: 1.0\n target_joints: [85, 25, 90, 0, 68, 90]\n blend_radius:\
\ 0\nvelocity_scaling: 40\n"
- name: s7_right_arm_put_down_box
params: "arm_motion_params:\n- arm_id: 1\n motion_type: 0\n target_pose:\n \
\ position:\n x: 0.0\n y: 0.0\n z: 0.0\n orientation:\n \
\ x: 0.0\n y: 0.0\n z: 0.0\n w: 1.0\n target_joints: [85.78, -88.95, 13.97, 22.88, -16.29, 67.99]\n blend_radius:\
\ 0\nvelocity_scaling: 40\n"
- name: s8_gripper_open
params: *gripper_open
- name: s9_right_arm_grasp_box
params: *grasp_box_s7_s9
- name: s10_move_waist_turn_right_90
params: *waist_turn_right_90
- name: s11_left_arm_initial
params: *left_arm_initial
- name: s11_right_arm_initial
params: *right_arm_initial
- name: s12_gripper_open
params: *gripper_open
- name: s13_left_arm_initial
params: *left_arm_initial
- name: s13_right_arm_initial
params: *right_arm_initial
- name: s14_left_arm_initial
params: *left_arm_initial
- name: s14_right_arm_initial
params: *right_arm_initial
- name: s15_left_arm_initial
params: *left_arm_initial
- name: s15_right_arm_initial
params: *right_arm_initial
- name: s16_left_arm_initial
params: *left_arm_initial
- name: s16_right_arm_initial
params: *right_arm_initial
- name: s14_gripper_open
params: *gripper_open
- name: s15_gripper_open
params: *gripper_open
- name: s16_gripper_open
params: *gripper_open
- name: s15_arm_pre_grasp
params: *pre_grasp
- name: s16_arm_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
- name: s6_move_waist_turn_right_90
params: *waist_turn_right_90
- name: s10_move_waist_turn_left_90
params: *waist_turn_left_90
- name: clear_done_instances_overall
params: '{}
'
- name: clear_done_instances_vision
params: '{}
'
- name: clear_done_instances_grasp
params: '{}
'
- name: clear_done_instances_putdown
params: '{}
'

196
src/brain/config/bt_vision_grasp_right_arm_demo1.xml Normal file
View File

@@ -0,0 +1,196 @@
<!--
功能描述: 右臂视觉抓取演示流程1（顶视 + 右手眼识别；含底盘与腰部转向；外层3次重试）。
运行概览:
1) 视觉阶段：双臂回初始并打开右爪（GripperCmd1）；底盘移动到取物位；顶视相机识别（最多10次）；
右臂到手眼拍照位后进行右手眼识别（最多10次）。
2) 抓取阶段：右臂预抓取 -> 右臂抓取 -> 关闭右爪 -> 右臂抬起 -> 左右臂并行保持抓箱位。
3) 放置阶段：底盘移动到放置位并腰部右转；左右臂并行抓箱位/放箱位 -> 右爪松开 -> 右臂回抓箱位；
腰部左转复位，同时双臂回初始位。
4) 恢复机制：视觉/抓取/放置各自失败时进入对应恢复子树（复位/重新拍照/清理完成标记），
并通过 AlwaysFailure 触发阶段重试。
-->
<root BTCPP_format="4" main_tree_to_execute="MainTree">
<BehaviorTree ID="MainTree">
<Sequence name="root">
<RetryUntilSuccessful name="retry_vision_grasp" num_attempts="3">
<Fallback name="fallback_main_action">
<Sequence name="complete_pick_and_place">
<!-- Vision with recovery -->
<Fallback name="vision_with_recovery">
<SubTree ID="vision_subtree"/>
<Sequence>
<SubTree ID="vision_recovery_subtree"/>
<AlwaysFailure/>
</Sequence>
</Fallback>
<!-- Grasp with recovery -->
<Fallback name="grasp_with_recovery">
<SubTree ID="grasp_subtree"/>
<Sequence>
<SubTree ID="grasp_recovery_subtree"/>
<AlwaysFailure/>
</Sequence>
</Fallback>
<!-- Putdown with recovery -->
<Fallback name="putdown_with_recovery">
<SubTree ID="putdown_subtree"/>
<Sequence>
<SubTree ID="putdown_recovery_subtree"/>
<AlwaysFailure/>
</Sequence>
</Fallback>
</Sequence>
<!-- Overall recovery if the entire sequence fails -->
<!-- <Sequence>
<SubTree ID="recovery_subtree"/>
<AlwaysFailure/>
</Sequence> -->
</Fallback>
</RetryUntilSuccessful>
</Sequence>
</BehaviorTree>
<BehaviorTree ID="vision_subtree">
<Sequence name="vision_root">
<RetryUntilSuccessful name="retry_vision" num_attempts="1">
<Sequence>
<Parallel name="parallel_action_1">
<DualArm_H name="s1_left_arm_initial" />
<DualArm_H name="s1_right_arm_initial" />
<GripperCmd1_H name="s1_gripper_open" />
</Parallel>
<Parallel name="parallel_action_2">
<MoveWheel_H name="s1_wheel_move_to_origin_pickup_position" />
<!-- <GripperCmd1_H name="s1_gripper_open" /> -->
<!-- <DualArm_H name="s2_left_arm_pre_cam_take_photo" />
<DualArm_H name="s2_right_arm_pre_cam_take_photo" /> -->
</Parallel>
<SubTree ID="vision_top_cam_subtree"/>
<DualArm_H name="s2_right_arm_cam_take_photo" />
<SubTree ID="vision_hand_cam_subtree"/>
</Sequence>
</RetryUntilSuccessful>
</Sequence>
</BehaviorTree>
<BehaviorTree ID="vision_top_cam_subtree">
<Sequence name="vision_top_cam_root">
<RetryUntilSuccessful name="retry_top_cam_vision_recg_1" num_attempts="10">
<VisionObjectRecognition_H name="s2_top_camera_vision_recg" />
</RetryUntilSuccessful>
</Sequence>
</BehaviorTree>
<BehaviorTree ID="vision_hand_cam_subtree">
<Sequence name="vision_hand_cam_root">
<RetryUntilSuccessful name="retry_hand_cam_vision_recg_1" num_attempts="10">
<VisionObjectRecognition_H name="s3_right_camera_vision_recg" />
</RetryUntilSuccessful>
</Sequence>
</BehaviorTree>
<BehaviorTree ID="grasp_subtree">
<Sequence name="grasp_root">
<RetryUntilSuccessful name="retry_grasp" num_attempts="1">
<Sequence>
<DualArm_H name="s3_right_arm_vision_pre_grasp" />
<DualArm_H name="s4_right_arm_vision_grasp" />
<GripperCmd1_H name="s4_gripper_close" />
<DualArm_H name="s4_right_arm_take_box" />
<Parallel name="parallel_action_grasp_box">
<DualArm_H name="s5_left_arm_grasp_box" />
<DualArm_H name="s5_right_arm_grasp_box" />
</Parallel>
</Sequence>
</RetryUntilSuccessful>
</Sequence>
</BehaviorTree>
<BehaviorTree ID="putdown_subtree">
<Sequence name="putdown_root">
<RetryUntilSuccessful name="retry_putdown" num_attempts="1">
<Sequence>
<Parallel name="parallel_action_2">
<MoveWheel_H name="s6_wheel_move_to_putdown_position" />
<MoveWaist_H name="s6_move_waist_turn_right_90" />
</Parallel>
<Parallel name="parallel_action_s7_grasp_box">
<DualArm_H name="s7_left_arm_grasp_box" />
<DualArm_H name="s7_right_arm_grasp_box" />
</Parallel>
<Parallel name="parallel_action_s7_put_down_box">
<DualArm_H name="s7_left_arm_put_down_box" />
<DualArm_H name="s7_right_arm_put_down_box" />
</Parallel>
<GripperCmd1_H name="s8_gripper_open" />
<DualArm_H name="s9_right_arm_grasp_box" />
<MoveWaist_H name="s10_move_waist_turn_left_90" />
<Parallel name="parallel_action_3">
<DualArm_H name="s11_right_arm_initial" />
<DualArm_H name="s11_left_arm_initial" />
</Parallel>
</Sequence>
</RetryUntilSuccessful>
</Sequence>
</BehaviorTree>
<BehaviorTree ID="recovery_subtree">
<Sequence name="recovery_root">
<RetryUntilSuccessful name="retry_recovery" num_attempts="1">
<Sequence>
<GripperCmd1_H name="s12_gripper_open" />
<DualArm_H name="s13_right_arm_initial" />
<DualArm_H name="s13_left_arm_initial" />
<ClearDoneInstances_H name="clear_done_instances_overall" />
</Sequence>
</RetryUntilSuccessful>
</Sequence>
</BehaviorTree>
<BehaviorTree ID="vision_recovery_subtree">
<Sequence name="vision_recovery_root">
<RetryUntilSuccessful name="retry_vision_recovery" num_attempts="1">
<Sequence>
<!-- <GripperCmd1_H name="s14_gripper_open" /> -->
<DualArm_H name="s14_right_arm_initial" />
<DualArm_H name="s14_left_arm_initial" />
<ClearDoneInstances_H name="clear_done_instances_vision" />
</Sequence>
</RetryUntilSuccessful>
</Sequence>
</BehaviorTree>
<BehaviorTree ID="grasp_recovery_subtree">
<Sequence name="grasp_recovery_root">
<RetryUntilSuccessful name="retry_grasp_recovery" num_attempts="1">
<Sequence>
<GripperCmd1_H name="s15_gripper_open" />
<DualArm_H name="s15_arm_pre_grasp" />
<DualArm_H name="s15_right_arm_cam_take_photo" />
<DualArm_H name="s15_right_arm_initial" />
<DualArm_H name="s15_left_arm_initial" />
<ClearDoneInstances_H name="clear_done_instances_grasp" />
</Sequence>
</RetryUntilSuccessful>
</Sequence>
</BehaviorTree>
<BehaviorTree ID="putdown_recovery_subtree">
<Sequence name="putdown_recovery_root">
<RetryUntilSuccessful name="retry_putdown_recovery" num_attempts="1">
<Sequence>
<GripperCmd1_H name="s16_gripper_open" />
<DualArm_H name="s16_arm_pre_grasp" />
<DualArm_H name="s16_right_arm_cam_take_photo" />
<DualArm_H name="s16_right_arm_initial" />
<DualArm_H name="s16_left_arm_initial" />
<ClearDoneInstances_H name="clear_done_instances_putdown" />
</Sequence>
</RetryUntilSuccessful>
</Sequence>
</BehaviorTree>
</root>

10
src/brain/config/robot_config.yaml
View File

@@ -9,7 +9,10 @@
{config: /config/bt_carry_boxes_sch2.xml, param: /config/bt_carry_boxes_sch2.params.yaml},
{config: /config/bt_vision_grasp_left_arm.xml, param: /config/bt_vision_grasp_left_arm.params.yaml},
{config: /config/bt_vision_grasp_right_arm.xml, param: /config/bt_vision_grasp_right_arm.params.yaml},
{config: /config/bt_vision_grasp_dual_arm.xml, param: /config/bt_vision_grasp_dual_arm.params.yaml}
{config: /config/bt_vision_grasp_dual_arm.xml, param: /config/bt_vision_grasp_dual_arm.params.yaml},
{config: /config/bt_vision_grasp_left_arm_demo1.xml, param: /config/bt_vision_grasp_left_arm_demo1.params.yaml},
{config: /config/bt_vision_grasp_right_arm_demo1.xml, param: /config/bt_vision_grasp_right_arm_demo1.params.yaml},
{config: /config/bt_vision_grasp_dual_arm_demo1.xml, param: /config/bt_vision_grasp_dual_arm_demo1.params.yaml}
]
- name: cerebellum_node
@@ -19,5 +22,8 @@
{config: /config/bt_carry_boxes_sch2.xml, param: /config/bt_carry_boxes_sch2.params.yaml},
{config: /config/bt_vision_grasp_left_arm.xml, param: /config/bt_vision_grasp_left_arm.params.yaml},
{config: /config/bt_vision_grasp_right_arm.xml, param: /config/bt_vision_grasp_right_arm.params.yaml},
{config: /config/bt_vision_grasp_dual_arm.xml, param: /config/bt_vision_grasp_dual_arm.params.yaml}
{config: /config/bt_vision_grasp_dual_arm.xml, param: /config/bt_vision_grasp_dual_arm.params.yaml},
{config: /config/bt_vision_grasp_left_arm_demo1.xml, param: /config/bt_vision_grasp_left_arm_demo1.params.yaml},
{config: /config/bt_vision_grasp_right_arm_demo1.xml, param: /config/bt_vision_grasp_right_arm_demo1.params.yaml},
{config: /config/bt_vision_grasp_dual_arm_demo1.xml, param: /config/bt_vision_grasp_dual_arm_demo1.params.yaml}
]

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

@@ -1044,16 +1044,35 @@ void CerebellumHooks::ConfigureVisionObjectRecognitionHooks(CerebellumNode * nod
return req;
}
// fallback to parameter
// const std::string param = "vision_object_recognition.camera_position";
// if (!node->has_parameter(param)) {
// node->declare_parameter<std::string>(param, std::string("right"));
// }
// req->camera_position = node->get_parameter(param).as_string();
normalize_camera_position(req->camera_position, skill_name);
node->camera_position_ = req->camera_position;
RCLCPP_INFO(node->get_logger(), "[%s] camera_position: %s", skill_name.c_str(), req->camera_position.c_str());
const bool classes_empty = req->classes.empty();
const bool classes_in_targets = (!classes_empty && !node->target_frames_.empty() &&
std::find(node->target_frames_.begin(), node->target_frames_.end(), req->classes[0]) != node->target_frames_.end());
if (classes_empty || !classes_in_targets) {
if (node->target_frames_.empty()) {
RCLCPP_WARN(node->get_logger(), "[%s] classes empty or not in target_frames_, and target_frames_ is empty",
skill_name.c_str());
req->classes.clear();
} else {
req->classes = node->target_frames_;
RCLCPP_INFO(node->get_logger(), "[%s] classes reset from target_frames_, size=%zu",
skill_name.c_str(), req->classes.size());
}
}
if (!req->classes.empty() &&
std::find(node->target_frames_.begin(), node->target_frames_.end(), req->classes[0]) != node->target_frames_.end()) {
node->target_frame_ = req->classes[0];
}
if (node->target_frame_.empty() && !node->target_frames_.empty()) {
node->target_frame_ = node->target_frames_.front();
RCLCPP_WARN(node->get_logger(), "[%s] target_frame empty, fallback to first target_frames_: %s",
skill_name.c_str(), node->target_frame_.c_str());
}
RCLCPP_INFO(node->get_logger(), "[%s] camera_position: %s, target_frame: %s",
skill_name.c_str(), req->camera_position.c_str(), node->target_frame_.c_str());
return req;
};