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

add wheel and waist action to sch

Browse Source
This commit is contained in:
NuoDaJia02
2026-01-12 10:13:19 +08:00
parent 0b6f18729c
commit 88dceb1a1a
3 changed files with 55 additions and 9 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -365,4 +365,30 @@
- name: s0_right_hand_gripper_open
params: *gripper_open
- name: s0_left_hand_gripper_open
params: *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

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

@@ -5,6 +5,9 @@
<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"/>
@@ -23,8 +26,23 @@
<!-- 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>
@@ -89,7 +107,7 @@
<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" />
<!-- <Arm_H name="s11_right_arm_initial" /> -->
</Sequence>
</RetryUntilSuccessful>
</Sequence>
@@ -126,7 +144,7 @@
<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" />
<!-- <Arm_H name="s11_left_arm_initial" /> -->
</Sequence>
</RetryUntilSuccessful>
</Sequence>
@@ -159,7 +177,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>
@@ -168,7 +186,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>
@@ -213,6 +231,7 @@
<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>
<!-- 左臂 -->
@@ -221,6 +240,7 @@
<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>

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

@@ -657,8 +657,8 @@ 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.08f;
target_pose.pose.position.y = 0.30f;
target_pose.pose.position.x += 0.10f;
target_pose.pose.position.y = 0.32f;
// target_pose.pose.position.z += 0.03f;
target_pose.pose.orientation.x = -0.7071;
target_pose.pose.orientation.y = 0.0000;
@@ -676,8 +676,8 @@ void CerebellumHooks::ConfigureVisionObjectRecognitionHooks(CerebellumNode * nod
return false;
}
target_pose.pose = left_arm_pose.pose;
target_pose.pose.position.x += 0.08f;
target_pose.pose.position.y = -0.30f;
target_pose.pose.position.x += 0.10f;
target_pose.pose.position.y = -0.32f;
// target_pose.pose.position.z += 0.03f;
target_pose.pose.orientation.x = 0.7071;
target_pose.pose.orientation.y = 0.0000;