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

test oK

Browse Source
This commit is contained in:
NuoDaJia02
2025-12-25 19:01:01 +08:00
parent 7f05981c15
commit 994f2a574f
4 changed files with 131 additions and 112 deletions
Download Patch File Download Diff File Expand all files Collapse all files

174
src/brain/config/bt_vision_grasp.params.yaml
View File

@@ -5,6 +5,12 @@
- name: retry_vision_hand
params: '{}
'
- name: s1_move_waist_turn_right_90
params: 'move_pitch_degree: 0.0
move_yaw_degree: -90.0
'
- name: s1_gripper_open
params: 'loc: 0
@@ -15,6 +21,20 @@
mode: 2
'
- name: s1_right_arm_pre_cam_take_photo
params: 'body_id: 2
data_type: 2
data_length: 12
command_id: 0
frame_time_stamp: 0
data_array: [110.38, 24.02, 89.51, -175.36, -65.38, -70.06, 60.27, -20.97, -87.36, 5.58, -68.56, 58.21]
'
- name: s2_top_camera_vision_recg
params: 'camera_position: top
@@ -67,16 +87,64 @@
'
- name: s4_gripper_close
params: 'loc: 90
params: 'loc: 200
speed: 20
speed: 150
torque: 20
torque: 100
mode: 2
mode: 1
'
- name: s4_gripper_open
- name: s4_right_arm_take_box
params: 'body_id: 1
data_type: 110
data_length: 0
command_id: 0
frame_time_stamp: 0
data_array: []
'
- name: s5_right_arm_grasp_box
params: 'body_id: 2
data_type: 2
data_length: 12
command_id: 0
frame_time_stamp: 0
data_array: [89.83, 20.24, 95.26, -175.23, -65.09, -90.85, 96.30, -88.95, 13.98, -22.88, -16.30, 112.10]
'
- name: s6_move_waist_turn_left_180
params: 'move_pitch_degree: 0.0
move_yaw_degree: 180.0
'
- name: s7_right_arm_put_down_box
params: 'body_id: 2
data_type: 2
data_length: 12
command_id: 0
frame_time_stamp: 0
data_array: [89.83, 20.24, 95.26, -175.23, -65.09, -90.85, 83.78, -88.95, 13.97, 22.88, -16.29, 67.99]
'
- name: s8_gripper_open
params: 'loc: 0
speed: 20
@@ -86,97 +154,37 @@
mode: 2
'
- name: s5_right_arm_vision_pre_grasp
params: 'body_id: 1
- name: s9_right_arm_grasp_box
params: 'body_id: 2
data_type: 101
data_type: 2
data_length: 0
data_length: 12
command_id: 0
frame_time_stamp: 0
data_array: []
data_array: [89.83, 20.24, 95.26, -175.23, -65.09, -90.85, 96.30, -88.95, 13.98, -22.88, -16.30, 112.10]
'
- name: s10_move_waist_turn_right_90
params: 'move_pitch_degree: 0.0
move_yaw_degree: -90.0
'
- name: s6_right_arm_cam_take_photo
params: 'body_id: 1
- name: s11_right_arm_pre_cam_take_photo
params: 'body_id: 2
data_type: 100
data_type: 2
data_length: 0
data_length: 12
command_id: 0
frame_time_stamp: 0
data_array: []
'
- name: s7_right_arm_vision_pre_grasp
params: 'body_id: 1
data_type: 103
data_length: 0
command_id: 0
frame_time_stamp: 0
data_array: []
'
- name: s8_right_arm_vision_grasp
params: 'body_id: 1
data_type: 104
data_length: 0
command_id: 0
frame_time_stamp: 0
data_array: []
'
- name: s9_right_arm_vision_pre_grasp
params: 'body_id: 1
data_type: 103
data_length: 0
command_id: 0
frame_time_stamp: 0
data_array: []
'
- name: s10_right_arm_cam_take_photo
params: 'body_id: 1
data_type: 90
data_length: 0
command_id: 0
frame_time_stamp: 0
data_array: []
'
- name: gripper_close
params: 'loc: 98
speed: 20
torque: 20
mode: 2
'
data_array: [110.38, 24.02, 89.51, -175.36, -65.38, -70.06, 60.27, -20.97, -87.36, 5.58, -68.56, 58.21]
'

27
src/brain/config/bt_vision_grasp.xml
View File

@@ -1,30 +1,33 @@
<root BTCPP_format="4">
<BehaviorTree ID="MainTree">
<Sequence name="root">
<!-- Retry vision + hand until GripperControl_H succeeds -->
<RetryUntilSuccessful name="retry_vision_hand" num_attempts="3">
<Sequence>
<GripperCmd_H name="s1_gripper_open" />
<Arm_H name="s1_right_arm_pre_cam_take_photo" />
<Parallel name="parallel_action_1">
<MoveWaist_H name="s1_move_waist_turn_right_90" />
<GripperCmd_H name="s1_gripper_open" />
</Parallel>
<!-- <Arm_H name="s1_right_arm_pre_cam_take_photo" /> -->
<VisionObjectRecognition_H name="s2_top_camera_vision_recg" />
<Arm_H name="s2_right_arm_cam_take_photo" />
<VisionObjectRecognition_H name="s3_right_camera_vision_recg" />
<Arm_H name="s3_right_arm_vision_pre_grasp" />
<Arm_H name="s4_right_arm_vision_grasp" />
<GripperCmd_H name="s4_gripper_close" />
<GripperCmd_H name="s4_gripper_open" />
<Arm_H name="s5_right_arm_vision_pre_grasp" />
<Arm_H name="s6_right_arm_cam_take_photo" />
<Arm_H name="s4_right_arm_take_box" />
<!-- <Arm_H name="s7_right_arm_vision_pre_grasp" />
<Arm_H name="s8_right_arm_vision_grasp" />
<Arm_H name="s9_right_arm_vision_pre_grasp" />
<Arm_H name="s10_right_arm_cam_take_photo" /> -->
<Arm_H name="s5_right_arm_grasp_box" />
<!-- <GripperCmd_H name="gripper_close" /> -->
<MoveWaist_H name="s6_move_waist_turn_left_180" />
<Arm_H name="s7_right_arm_put_down_box" />
<GripperCmd_H name="s8_gripper_open" />
<Arm_H name="s9_right_arm_grasp_box" />
<MoveWaist_H name="s10_move_waist_turn_right_90" />
<Arm_H name="s11_right_arm_pre_cam_take_photo" />
</Sequence>
</RetryUntilSuccessful>
<!-- After Arm_H eventually succeeds, run the hand -->
<!-- <GripperCmd_H name="GripperCmd_H" /> -->
</Sequence>
</BehaviorTree>
</root>

3
src/brain/include/brain/cerebellum_node.hpp
View File

@@ -43,9 +43,10 @@ public:
*/
explicit CerebellumNode(const rclcpp::NodeOptions & options = rclcpp::NodeOptions());
geometry_msgs::msg::Pose right_hand_take_photo_pose_;
std::vector<geometry_msgs::msg::Pose> right_hand_take_photo_pose_;
std::vector<geometry_msgs::msg::Pose> pre_grasp_poses_;
std::vector<geometry_msgs::msg::Pose> grasp_poses_;
geometry_msgs::msg::Pose right_hand_take_object_pose_;
std::vector<double> left_arm_joint_angles_;
std::vector<double> right_arm_joint_angles_;

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

@@ -82,19 +82,26 @@ void CerebellumHooks::ConfigureArmHooks(CerebellumNode * node)
auto angle = node->right_arm_cam_joint_angles_;
goal.data_array = {angle[0], angle[1], angle[2], angle[3], angle[4], angle[5]};
goal.data_length = USED_ARM_DOF;
} else if (goal.data_type >= 100 && goal.data_type < 110) {
} else if (goal.data_type >= 100 && goal.data_type <= 110) {
geometry_msgs::msg::Pose pose;
if (goal.data_type == 100) {
pose = node->right_hand_take_photo_pose_;
pose = node->right_hand_take_photo_pose_[0];
}
else if (goal.data_type == 101) { // pre-grasp
pose = node->pre_grasp_poses_[0];
} else if (goal.data_type == 102) { // grasp
pose = node->grasp_poses_[0];
pose.position.y += 0.03;
node->right_hand_take_object_pose_ = pose;
node->right_hand_take_object_pose_.position.x -= 0.06;
} else if (goal.data_type == 103) { // left arm pre-grasp
pose = node->pre_grasp_poses_[1];
} else if (goal.data_type == 104) { // left arm grasp
pose = node->grasp_poses_[1];
} else if (goal.data_type == 110) {
pose = node->right_hand_take_object_pose_;
} else {
RCLCPP_ERROR(node->get_logger(), "ARM make goal error: invalid data_type");
return goal;
@@ -588,26 +595,25 @@ void CerebellumHooks::ConfigureVisionObjectRecognitionHooks(CerebellumNode * nod
pose.position.x, pose.position.y, pose.position.z, pose.orientation.x,
pose.orientation.y, pose.orientation.z, pose.orientation.w);
node->right_hand_take_photo_pose_.position.x = pose.position.x + 0.04f;
node->right_hand_take_photo_pose_.position.y = pose.position.y - 0.40f;
node->right_hand_take_photo_pose_.position.z = pose.position.z;
node->right_hand_take_photo_pose_.orientation.x = -0.7071;
node->right_hand_take_photo_pose_.orientation.y = 0.0000;
node->right_hand_take_photo_pose_.orientation.z = -0.0000;
node->right_hand_take_photo_pose_.orientation.w = 0.7071;
pose.position.x += 0.08f;
pose.position.y -= 0.40f;
pose.orientation.x = -0.7071;
pose.orientation.y = 0.0000;
pose.orientation.z = -0.0000;
pose.orientation.w = 0.7071;
RCLCPP_INFO(node->get_logger(), "cam take photo pose: %.4f, %.4f, %.4f, %.4f, %.4f, %.4f, %.4f",
node->right_hand_take_photo_pose_.position.x, node->right_hand_take_photo_pose_.position.y,
node->right_hand_take_photo_pose_.position.z, node->right_hand_take_photo_pose_.orientation.x,
node->right_hand_take_photo_pose_.orientation.y, node->right_hand_take_photo_pose_.orientation.z,
node->right_hand_take_photo_pose_.orientation.w);
pose.position.x, pose.position.y, pose.position.z, pose.orientation.x,
pose.orientation.y, pose.orientation.z, pose.orientation.w);
if (node->right_hand_take_photo_pose_.position.x < 0.1 ||
node->right_hand_take_photo_pose_.position.y < 0.1 || node->right_hand_take_photo_pose_.position.z < 0.15) {
if (pose.position.x < 0.1 || pose.position.y < 0.1 || pose.position.z < 0.15 ||
pose.position.x > 0.8 || pose.position.y > 0.8 || pose.position.z > 1.0) {
RCLCPP_ERROR(node->get_logger(), "invalid pose value, cal grasp pose failed");
return false;
}
node->right_hand_take_photo_pose_.push_back(pose);
return true;
}
@@ -662,7 +668,8 @@ void CerebellumHooks::ConfigureVisionObjectRecognitionHooks(CerebellumNode * nod
RCLCPP_INFO(node->get_logger(), "[%s] angle %f, palm %s, pre_grasp_pose[%.4f, %.4f, %.4f, %.4f, %.4f, %.4f, %.4f]",
skill_name.c_str(), angle, palm.c_str(),
result.pre_grasp_pose.position.x(), result.pre_grasp_pose.position.y(), result.pre_grasp_pose.position.z(),
result.pre_grasp_pose.orientation.x(), result.pre_grasp_pose.orientation.y(), result.pre_grasp_pose.orientation.z(), result.pre_grasp_pose.orientation.w());
result.pre_grasp_pose.orientation.x(), result.pre_grasp_pose.orientation.y(), result.pre_grasp_pose.orientation.z(),
result.pre_grasp_pose.orientation.w());
#endif
if (brain::GraspPoseCalculator::rm_arm_inverse_kinematics(result.pre_grasp_pose.position,