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

modify arm position

Browse Source
This commit is contained in:
NuoDaJia02
2026-01-08 10:08:47 +08:00
parent ac6bf49e75
commit 5c3994da9d
4 changed files with 115 additions and 62 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -26,8 +26,8 @@
params: *gripper_open
- name: s1_left_hand_gripper_open
params: *gripper_open
- name: s1_dual_arm_initial
params: &pre_cam_take_photo 'body_id: 2
- name: s1_right_arm_initial
params: 'body_id: 1
data_type: 2
@@ -37,17 +37,11 @@
frame_time_stamp: 0
data_array: [85, 24, 88, -175, -68, -90, 95, -24, -88, -5, -68, 90]
data_array: [0, 0, 0, 0, 0, 0, 95, -25, -90, 0, -68, 90]
'
- name: s1_wheel_move_to_origin_pickup_position
params: 'move_distance: 0.5
move_angle: 0.0
'
- name: s2_dual_arm_pre_cam_take_photo
params: 'body_id: 2
- name: s1_left_arm_initial
params: 'body_id: 0
data_type: 2
@@ -57,26 +51,44 @@
frame_time_stamp: 0
data_array: [120, 20, 80, 0, 80, 120, 60, -20, -80, 0, -80, 60]
data_array: [85, 25, 90, 0, 68, 90, 0, 0, 0, 0, 0, 0]
'
- name: s1_right_arm_pre_cam_take_photo
params: &pre_cam_take_photo_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, 60, -20, -80, 0, -80, 60]
'
- name: s1_left_arm_pre_cam_take_photo
params: &pre_cam_take_photo_left '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_pre_cam_take_photo
params: *pre_cam_take_photo_right
- name: s2_left_arm_pre_cam_take_photo
params: *pre_cam_take_photo_left
- name: s2_top_camera_vision_recg
params: 'camera_position: top
'
- name: s2_dual_arm_cam_take_photo
params: 'body_id: 2
data_type: 2
data_length: 12
command_id: 0
frame_time_stamp: 0
data_array: [120.0, 20.0, 80.0, 0.0, 80.0, 120.0, 60.0, -20.0, -80.0, 0.0, -80.0, 60.0]
'
- name: s3_right_camera_vision_recg
params: 'camera_position: right
@@ -182,8 +194,8 @@
data_array: []
'
- name: s5_right_arm_grasp_box
params: 'body_id: 2
- name: s5_right_arm_take_box_off
params: 'body_id: 1
data_type: 2
@@ -193,11 +205,11 @@
frame_time_stamp: 0
data_array: [85, 24, 88, -175, -68, -90, 96.30, -88.95, -45, -22.88, -16.30, 112.10]
data_array: [0, 0, 0, 0, 0, 0, 96.31, -103.06, 17.69, -53.40, -7.81, 143.19]
'
- name: s5_left_arm_grasp_box
params: 'body_id: 2
- name: s5_left_arm_take_box_off
params: 'body_id: 0
data_type: 2
@@ -207,11 +219,11 @@
frame_time_stamp: 0
data_array: [96.30, -88.95, -45, -22.88, -16.30, 112.10, 85, 24, 88, -175, -68, -90]
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: 2
params: &grasp_box_s7_s9_right 'body_id: 1
data_type: 2
@@ -221,11 +233,11 @@
frame_time_stamp: 0
data_array: [85, 24, 88, -175, -68, -90, 96.30, -88.95, 13.98, -22.88, -16.30, 112.10]
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: 2
params: &grasp_box_s7_s9_left 'body_id: 0
data_type: 2
@@ -235,11 +247,11 @@
frame_time_stamp: 0
data_array: [96.30, -88.95, 13.98, -22.88, -16.30, 112.10, 85, 24, 88, -175, -68, -90]
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: 2
params: 'body_id: 1
data_type: 2
@@ -249,11 +261,11 @@
frame_time_stamp: 0
data_array: [85, 24, 88, -175, -68, -90, 85.78, -88.95, 13.97, 22.88, -16.29, 67.99]
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: 2
params: 'body_id: 0
data_type: 2
@@ -263,7 +275,7 @@
frame_time_stamp: 0
data_array: [85.78, -88.95, 13.97, 22.88, -16.29, 67.99, 85, 24, 88, -175, -68, -90]
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
@@ -275,17 +287,37 @@
- name: s9_left_arm_grasp_box
params: *grasp_box_s7_s9_left
- name: s11_right_arm_pre_cam_take_photo
params: *pre_cam_take_photo
params: *pre_cam_take_photo_right
- name: s11_left_arm_pre_cam_take_photo
params: *pre_cam_take_photo
params: *pre_cam_take_photo_left
- name: s12_right_hand_gripper_open
params: *gripper_open
- name: s12_left_hand_gripper_open
params: *gripper_open
- name: s13_dual_arm_pre_cam_take_photo
params: *pre_cam_take_photo
- name: s14_dual_arm_pre_cam_take_photo
params: *pre_cam_take_photo
- name: s13_right_arm_pre_cam_take_photo
params: *pre_cam_take_photo_right
- name: s13_left_arm_pre_cam_take_photo
params: *pre_cam_take_photo_left
- name: clear_done_instances_overall
params: '{}
'
- name: s14_right_arm_pre_cam_take_photo
params: *pre_cam_take_photo_right
- name: s14_left_arm_pre_cam_take_photo
params: *pre_cam_take_photo_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
@@ -295,8 +327,8 @@
- name: s15_left_arm_pre_grasp
params: *pre_grasp_left
- name: s15_right_arm_pre_cam_take_photo
params: *pre_cam_take_photo
params: *pre_cam_take_photo_right
- name: s15_left_arm_pre_cam_take_photo
params: *pre_cam_take_photo
params: *pre_cam_take_photo_left
- name: s16_right_arm_pre_cam_take_photo
params: *pre_cam_take_photo
params: *pre_cam_take_photo_right

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

@@ -58,17 +58,24 @@
<!-- Shared setup and head-camera vision -->
<BehaviorTree ID="vision_setup_subtree">
<Sequence name="vision_setup_root">
<Arm_H name="s1_dual_arm_initial" />
<Parallel name="parallel_arm_init_action">
<Arm_H name="s1_right_arm_initial" />
<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" />
<Arm_H name="s2_dual_arm_pre_cam_take_photo" />
<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 -->
<Arm_H name="s2_dual_arm_cam_take_photo" />
<Parallel name="arm_pre_cam_take_photo1">
<Arm_H name="s2_right_arm_pre_cam_take_photo" />
<Arm_H name="s2_left_arm_pre_cam_take_photo" />
</Parallel>
</Sequence>
</BehaviorTree>
@@ -89,7 +96,7 @@
<Arm_H name="s4_right_arm_vision_grasp" />
<GripperCmd0_H name="s4_right_hand_gripper_close" />
<Arm_H name="s4_right_arm_take_box" />
<Arm_H name="s5_right_arm_grasp_box" />
<Arm_H name="s5_right_arm_take_box_off" />
</Sequence>
</RetryUntilSuccessful>
</Sequence>
@@ -99,7 +106,7 @@
<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_grasp_box" /> -->
<Arm_H name="s7_right_arm_put_down_box" />
<GripperCmd0_H name="s8_right_hand_gripper_open" />
<Arm_H name="s9_right_arm_grasp_box" />
@@ -126,7 +133,7 @@
<Arm_H name="s4_left_arm_vision_grasp" />
<GripperCmd1_H name="s4_left_hand_gripper_close" />
<Arm_H name="s4_left_arm_take_box" />
<Arm_H name="s5_left_arm_grasp_box" />
<Arm_H name="s5_left_arm_take_box_off" />
</Sequence>
</RetryUntilSuccessful>
</Sequence>
@@ -136,7 +143,7 @@
<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_grasp_box" /> -->
<Arm_H name="s7_left_arm_put_down_box" />
<GripperCmd1_H name="s8_left_hand_gripper_open" />
<Arm_H name="s9_left_arm_grasp_box" />
@@ -152,15 +159,19 @@
<Parallel name="overall_recovery_parallel">
<GripperCmd0_H name="s12_right_hand_gripper_open" />
<GripperCmd1_H name="s12_left_hand_gripper_open" />
<Arm_H name="s13_right_arm_pre_cam_take_photo" />
<Arm_H name="s13_left_arm_pre_cam_take_photo" />
</Parallel>
<Arm_H name="s13_dual_arm_pre_cam_take_photo" />
<ClearDoneInstances_H name="clear_done_instances_overall" />
</Sequence>
</BehaviorTree>
<BehaviorTree ID="vision_recovery_subtree">
<Sequence name="vision_recovery_root">
<Arm_H name="s14_dual_arm_pre_cam_take_photo" />
<Parallel name="arm_pre_cam_take_photo2">
<Arm_H name="s14_right_arm_pre_cam_take_photo" />
<Arm_H name="s14_left_arm_pre_cam_take_photo" />
</Parallel>
<ClearDoneInstances_H name="clear_done_instances_vision" />
</Sequence>
</BehaviorTree>

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

@@ -7,7 +7,8 @@
config_params_path: [
{config: /config/bt_carry_boxes_sch1.xml, param: /config/bt_carry_boxes_sch1.params.yaml},
{config: /config/bt_carry_boxes_sch2.xml, param: /config/bt_carry_boxes_sch2.params.yaml},
{config: /config/bt_vision_grasp.xml, param: /config/bt_vision_grasp.params.yaml}
{config: /config/bt_vision_grasp.xml, param: /config/bt_vision_grasp.params.yaml},
{config: /config/bt_vision_grasp_dual_arm.xml, param: /config/bt_vision_grasp_dual_arm.params.yaml}
]
- name: cerebellum_node
@@ -15,5 +16,6 @@
config_params_path: [
{config: /config/bt_carry_boxes_sch1.xml, param: /config/bt_carry_boxes_sch1.params.yaml},
{config: /config/bt_carry_boxes_sch2.xml, param: /config/bt_carry_boxes_sch2.params.yaml},
{config: /config/bt_vision_grasp.xml, param: /config/bt_vision_grasp.params.yaml}
{config: /config/bt_vision_grasp.xml, param: /config/bt_vision_grasp.params.yaml},
{config: /config/bt_vision_grasp_dual_arm.xml, param: /config/bt_vision_grasp_dual_arm.params.yaml}
]

12
src/brain/src/cerebrum_node.cpp
View File

@@ -1199,6 +1199,7 @@ void CerebrumNode::HandleTriggerRebuild(
return;
}
bool match_found = false;
for (const auto & path_param : bt_config_params_paths_) {
// path_param is a pair {config, param}; compare the config path string against the current path
// if (path_param.config == current_bt_config_params_path_.config) {
@@ -1221,11 +1222,18 @@ void CerebrumNode::HandleTriggerRebuild(
}
RCLCPP_WARN(this->get_logger(), "cerebrum/rebuild_now Service Switching to BT config file path: %s", path_param.config.c_str());
match_found = true;
break;
}
resp->success = true;
resp->message = "Rebuild triggered";
if (match_found) {
resp->success = true;
resp->message = "Rebuild triggered";
} else {
resp->success = false;
resp->message = "Rebuild config not found";
RCLCPP_ERROR(this->get_logger(), "cerebrum/rebuild_now Service Failed to find config: %s", req->config.c_str());
}
}
/**