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

test dual arm grasp sch ok

Browse Source
This commit is contained in:
NuoDaJia02
2026-01-11 14:26:00 +08:00
parent 2348200dc7
commit 385527dc2a
3 changed files with 135 additions and 54 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -1,7 +1,7 @@
<root BTCPP_format="4" main_tree_to_execute="MainTree">
<BehaviorTree ID="MainTree">
<Sequence name="root">
<Script code="right_arm_done:=false; left_arm_done:=false" />
<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">
@@ -14,21 +14,97 @@
</Sequence>
</Fallback>
<!-- Phase 2: Independent Dual Arm Operation -->
<!-- Asynchronous execution of both arms -->
<Parallel name="dual_arm_asynchronous_grasp" success_count="2" failure_count="2">
<!-- Right Arm Branch -->
<Sequence name="right_arm_branch">
<!-- 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>
<ScriptCondition code="right_arm_done==true" />
<Sequence>
<Fallback name="right_arm_vision_process_with_recovery">
<SubTree ID="right_arm_hand_vision_subtree"/>
<Sequence name="right_arm_cam_take_photo_branch">
<Fallback>
<Sequence>
<SubTree ID="right_arm_vision_recovery_subtree"/>
<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>
<!-- 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>
<!-- 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"/>
@@ -41,21 +117,14 @@
</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>
</Sequence>
<!-- Left Arm Branch -->
<Sequence name="left_arm_branch">
<Fallback>
<ScriptCondition code="left_arm_done==true" />
<Sequence>
<Fallback name="left_arm_vision_process_with_recovery">
<SubTree ID="left_arm_hand_vision_subtree"/>
<Sequence>
<SubTree ID="left_arm_vision_recovery_subtree"/>
<AlwaysFailure/>
</Sequence>
</Fallback>
<!-- 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"/>
@@ -68,13 +137,21 @@
</Fallback>
<Script code="left_arm_done:=true" />
</Sequence>
<AlwaysSuccess name="force_wait_left"/>
</Fallback>
</Sequence>
</Parallel>
</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>
</Sequence>
<!-- Overall fallback recovery -->
<SubTree ID="recovery_subtree"/>
<Sequence>
<!-- Overall fallback recovery -->
<SubTree ID="recovery_subtree"/>
<AlwaysFailure/>
</Sequence>
</Fallback>
</RetryUntilSuccessful>
</Sequence>
@@ -88,19 +165,14 @@
<Arm_H name="s1_left_arm_initial" />
</Parallel>
<Parallel name="parallel_init_action">
<GripperCmd0_H name="s1_right_hand_gripper_open" />
<GripperCmd1_H name="s1_left_hand_gripper_open" />
<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="10">
<VisionObjectRecognition_H name="s2_top_camera_vision_recg" />
</RetryUntilSuccessful>
<!-- Move both arms to positions where their hand cameras can see potential objects -->
<Parallel name="arm_cam_take_photo1">
<Arm_H name="s2_right_arm_cam_take_photo" />
<Arm_H name="s2_left_arm_cam_take_photo" />
</Parallel>
</Sequence>
</BehaviorTree>
@@ -119,7 +191,7 @@
<Sequence>
<Arm_H name="s3_right_arm_vision_pre_grasp" />
<Arm_H name="s4_right_arm_vision_grasp" />
<GripperCmd0_H name="s4_right_hand_gripper_close" />
<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>
@@ -133,7 +205,7 @@
<Sequence>
<!-- <Arm_H name="s7_right_arm_grasp_box" /> -->
<Arm_H name="s7_right_arm_put_down_box" />
<GripperCmd0_H name="s8_right_hand_gripper_open" />
<GripperCmd1_H name="s8_right_hand_gripper_open" />
<Arm_H name="s9_right_arm_grasp_box" />
<Arm_H name="s11_right_arm_initial" />
</Sequence>
@@ -156,7 +228,7 @@
<Sequence>
<Arm_H name="s3_left_arm_vision_pre_grasp" />
<Arm_H name="s4_left_arm_vision_grasp" />
<GripperCmd1_H name="s4_left_hand_gripper_close" />
<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>
@@ -170,7 +242,7 @@
<Sequence>
<!-- <Arm_H name="s7_left_arm_grasp_box" /> -->
<Arm_H name="s7_left_arm_put_down_box" />
<GripperCmd1_H name="s8_left_hand_gripper_open" />
<GripperCmd0_H name="s8_left_hand_gripper_open" />
<Arm_H name="s9_left_arm_grasp_box" />
<Arm_H name="s11_left_arm_initial" />
</Sequence>
@@ -182,8 +254,8 @@
<BehaviorTree ID="recovery_subtree">
<Sequence name="recovery_root">
<Parallel name="overall_recovery_parallel">
<GripperCmd0_H name="s12_right_hand_gripper_open" />
<GripperCmd1_H name="s12_left_hand_gripper_open" />
<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>
@@ -203,7 +275,7 @@
<BehaviorTree ID="right_arm_grasp_recovery_subtree">
<Sequence name="right_grasp_recovery_root">
<GripperCmd0_H name="s15_right_hand_gripper_open" />
<GripperCmd1_H name="s15_right_hand_gripper_open" />
<Arm_H name="s15_right_arm_pre_grasp" />
<Arm_H name="s15_right_arm_initial" />
<ClearDoneInstances_H name="clear_done_instances_right_grasp" />
@@ -212,7 +284,7 @@
<BehaviorTree ID="left_arm_grasp_recovery_subtree">
<Sequence name="left_grasp_recovery_root">
<GripperCmd1_H name="s15_left_hand_gripper_open" />
<GripperCmd0_H name="s15_left_hand_gripper_open" />
<Arm_H name="s15_left_arm_pre_grasp" />
<Arm_H name="s15_left_arm_initial" />
<ClearDoneInstances_H name="clear_done_instances_left_grasp" />
@@ -246,4 +318,5 @@
<ClearDoneInstances_H name="clear_done_instances_right_arm_vision" />
</Sequence>
</BehaviorTree>
</root>

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

@@ -49,7 +49,7 @@
<Arm_H name="s1_right_arm_pre_cam_take_photo" />
<Parallel name="parallel_action_1">
<!-- <MoveWheel_H name="s1_wheel_move_to_origin_pickup_position" /> -->
<GripperCmd0_H name="s1_gripper_open" />
<GripperCmd1_H name="s1_gripper_open" />
<Arm_H name="s2_right_arm_pre_cam_take_photo" />
</Parallel>
<SubTree ID="vision_top_cam_subtree"/>
@@ -83,7 +83,7 @@
<Sequence>
<Arm_H name="s3_right_arm_vision_pre_grasp" />
<Arm_H name="s4_right_arm_vision_grasp" />
<GripperCmd0_H name="s4_gripper_close" />
<GripperCmd1_H name="s4_gripper_close" />
<Arm_H name="s4_right_arm_take_box" />
<Arm_H name="s5_right_arm_grasp_box" />
</Sequence>
@@ -101,7 +101,7 @@
</Parallel> -->
<Arm_H name="s7_right_arm_grasp_box" />
<Arm_H name="s7_right_arm_put_down_box" />
<GripperCmd0_H name="s8_gripper_open" />
<GripperCmd1_H name="s8_gripper_open" />
<Arm_H name="s9_right_arm_grasp_box" />
<Parallel name="parallel_action_3">
@@ -117,7 +117,7 @@
<Sequence name="recovery_root">
<RetryUntilSuccessful name="retry_recovery" num_attempts="1">
<Sequence>
<GripperCmd0_H name="s12_gripper_open" />
<GripperCmd1_H name="s12_gripper_open" />
<Arm_H name="s13_right_arm_pre_cam_take_photo" />
<ClearDoneInstances_H name="clear_done_instances_overall" />
</Sequence>
@@ -129,7 +129,7 @@
<Sequence name="vision_recovery_root">
<RetryUntilSuccessful name="retry_vision_recovery" num_attempts="1">
<Sequence>
<!-- <GripperCmd0_H name="s14_gripper_open" /> -->
<!-- <GripperCmd1_H name="s14_gripper_open" /> -->
<Arm_H name="s14_right_arm_pre_cam_take_photo" />
<ClearDoneInstances_H name="clear_done_instances_vision" />
</Sequence>
@@ -141,7 +141,7 @@
<Sequence name="grasp_recovery_root">
<RetryUntilSuccessful name="retry_grasp_recovery" num_attempts="1">
<Sequence>
<GripperCmd0_H name="s15_gripper_open" />
<GripperCmd1_H name="s15_gripper_open" />
<Arm_H name="s15_arm_pre_grasp" />
<Arm_H name="s15_right_arm_pre_cam_take_photo" />
<ClearDoneInstances_H name="clear_done_instances_grasp" />
@@ -154,7 +154,7 @@
<Sequence name="putdown_recovery_root">
<RetryUntilSuccessful name="retry_putdown_recovery" num_attempts="1">
<Sequence>
<GripperCmd0_H name="s16_gripper_open" />
<GripperCmd1_H name="s16_gripper_open" />
<Arm_H name="s16_arm_pre_grasp" />
<Arm_H name="s16_right_arm_pre_cam_take_photo" />
<ClearDoneInstances_H name="clear_done_instances_putdown" />

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

@@ -81,7 +81,7 @@ void CerebellumHooks::ConfigureArmHooks(CerebellumNode * node)
}
} else if (goal.data_type >= 100 && goal.data_type <= 110) {
geometry_msgs::msg::Pose pose;
std::vector<double> angle{0.0, 0.0, 0.0, 0.0, 0.0, 0.0};
std::vector<double> angle{360.0, 360.0, 360.0, 360.0, 360.0, 360.0}; //invalid angles
if (goal.data_type == 100) { //take photo
if (goal.body_id == RIGHT_ARM) {
if (!node->right_arm_take_photo_poses_.empty()) {
@@ -659,7 +659,7 @@ void CerebellumHooks::ConfigureVisionObjectRecognitionHooks(CerebellumNode * nod
target_arm = "right_arm";
target_pose.pose.position.x += 0.08f;
target_pose.pose.position.y = 0.30f;
target_pose.pose.position.z += 0.03f;
// target_pose.pose.position.z += 0.03f;
target_pose.pose.orientation.x = -0.7071;
target_pose.pose.orientation.y = 0.0000;
target_pose.pose.orientation.z = -0.0000;
@@ -678,7 +678,7 @@ void CerebellumHooks::ConfigureVisionObjectRecognitionHooks(CerebellumNode * nod
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.z += 0.03f;
// target_pose.pose.position.z += 0.03f;
target_pose.pose.orientation.x = 0.7071;
target_pose.pose.orientation.y = 0.0000;
target_pose.pose.orientation.z = -0.0000;
@@ -793,6 +793,14 @@ void CerebellumHooks::ConfigureVisionObjectRecognitionHooks(CerebellumNode * nod
result.grasp_pose.position.x(), result.grasp_pose.position.y(), result.grasp_pose.position.z(),
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); //补偿抓取的偏差
}
//pre grasp
std::vector<double> pre_grasp_joint_angles = current_joint_angles;
if (brain::GraspPoseCalculator::rm_arm_inverse_kinematics(result.pre_grasp_pose.position,