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

add cali

Browse Source
This commit is contained in:
NuoDaJia02
2025-12-19 10:20:52 +08:00
parent 0ced39b18d
commit 75b2dd5d3f
5 changed files with 99 additions and 21 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -76,6 +76,62 @@
data_array: []
'
- name: s7_right_arm_vision_pre_grasp
params: 'body_id: 1
data_type: 102
data_length: 0
command_id: 0
frame_time_stamp: 0
data_array: []
'
- name: s8_right_arm_vision_grasp
params: 'body_id: 1
data_type: 103
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: 102
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

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

@@ -4,12 +4,18 @@
<!-- Retry vision + hand until GripperControl_H succeeds -->
<RetryUntilSuccessful name="retry_vision_hand" num_attempts="3">
<Sequence>
<GripperCmd_H name="s1_gripper_open" />
<!-- <GripperCmd_H name="s1_gripper_open" /> -->
<VisionObjectRecognition_H name="s2_VisionObjectRecognition_H" />
<Arm_H name="s3_right_arm_vision_pre_grasp" />
<Arm_H name="s4_right_arm_vision_grasp" />
<Arm_H name="s5_right_arm_vision_pre_grasp" />
<Arm_H name="s6_right_arm_cam_take_photo" />
<!-- <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" /> -->
<!-- <GripperCmd_H name="gripper_close" /> -->
</Sequence>
</RetryUntilSuccessful>

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

@@ -95,8 +95,8 @@ private:
std::mutex stats_mutex_;
rclcpp::Publisher<std_msgs::msg::String>::SharedPtr stats_pub_;
std::unordered_map<std::string, geometry_msgs::msg::PoseStamped> target_pose_;
std::vector<std::string> target_frames_{"apple", "bottle"};
std::string target_frame_{"bottle"};
std::vector<std::string> target_frames_{"apple", "bottle", "medicine_box"};
std::string target_frame_{"medicine_box"};
int64_t command_id_{0};
int64_t command_id_left_arm_ {0};
int64_t command_id_right_arm_{0};
@@ -108,7 +108,7 @@ private:
rclcpp::Subscription<sensor_msgs::msg::JointState>::SharedPtr joint_state_sub_;
void JointStateCallback(const sensor_msgs::msg::JointState::SharedPtr msg);
double grab_width_{0.0};
std::vector<double> grab_width_;
// Current accepted sequence epoch (from client) used to filter/reject stale goals
std::atomic<uint32_t> current_epoch_{0};

5
src/brain/src/calc_grasp_pose.cpp
View File

@@ -77,6 +77,11 @@ GraspResult GraspPoseCalculator::calculate(
tf2::Vector3 P_shoulder(0, 0, 0);
tf2::Vector3 V_sb = target_pos - P_shoulder;
tf2::Vector3 V_horiz = V_sb - (V_sb.dot(V_world_up) * V_world_up);
if (V_horiz.length() < 1e-3) {
V_horiz = tf2::Vector3(0.0, 1.0, 0.0); // Default horizontal vector
} else {
V_horiz = normalize_vector(V_horiz);
}
tf2::Vector3 approach_vec;
tf2::Vector3 palm_normal;
std::string name_suffix;

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

@@ -82,20 +82,25 @@ 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) { // 100: Set arm to pre-grasp pose
// pre-grasp pose handled in hooks.on_goal_response
} else if (goal.data_type >= 100 && goal.data_type < 110) {
geometry_msgs::msg::Pose pose;
if (goal.data_type == 100) { // pre-grasp
pose = node->pre_grasp_poses_[0];
} else if (goal.data_type == 101) { // grasp
pose = node->grasp_poses_[0];
} else if (goal.data_type == 102) { // left arm pre-grasp
pose = node->pre_grasp_poses_[1];
} else if (goal.data_type == 103) { // left arm grasp
pose = node->grasp_poses_[1];
} else {
RCLCPP_ERROR(node->get_logger(), "ARM make goal error: invalid data_type");
return goal;
}
goal.data_type = ARM_COMMAND_TYPE_POSE_DIRECT_MOVE;
auto pose = node->pre_grasp_poses_[0];
goal.data_length = POSE_DIMENSION;
goal.data_array = {pose.position.x, pose.position.y, pose.position.z,
pose.orientation.x, pose.orientation.y, pose.orientation.z, pose.orientation.w};
goal.data_length = POSE_DIMENSION;
} else if (goal.data_type == 101) { // 101: Set arm to grasp pose
// grasp pose handled in hooks.on_goal_response
goal.data_type = ARM_COMMAND_TYPE_POSE_DIRECT_MOVE;
auto pose = node->grasp_poses_[0];
goal.data_array = {pose.position.x, pose.position.y, pose.position.z,
pose.orientation.x, pose.orientation.y, pose.orientation.z, pose.orientation.w};
goal.data_length = POSE_DIMENSION;
}
else {
RCLCPP_ERROR(node->get_logger(), "ARM make goal error: data_type[%d] not support", goal.data_type);
@@ -548,12 +553,18 @@ void CerebellumHooks::ConfigureVisionObjectRecognitionHooks(CerebellumNode * nod
node->target_pose_[tf].header = header;
node->target_pose_[tf].header.frame_id = "RightLink6";
node->target_pose_[tf].pose = obj.pose;
node->grab_width_ = obj.grab_width;
node->grab_width_.clear();
for (int i = 0; i < obj.grab_width.size(); i++) {
node->grab_width_.push_back(obj.grab_width[i]);
RCLCPP_INFO(node->get_logger(), "grab_width : %.4f", obj.grab_width[i]);
}
const auto & p = obj.pose;
RCLCPP_INFO(node->get_logger(), "target_frame: %s, grab_width: %f, target_pose: %.4f, %.4f, %.4f, %.4f, %.4f, %.4f, %.4f",
tf.c_str(), node->grab_width_,
RCLCPP_INFO(node->get_logger(), "target_frame: %s, grab_width_size: %zu, target_pose: %.4f, %.4f, %.4f, %.4f, %.4f, %.4f, %.4f",
tf.c_str(), node->grab_width_.size(),
p.position.x, p.position.y, p.position.z, p.orientation.x, p.orientation.y, p.orientation.z, p.orientation.w);
return true;
} else {
RCLCPP_WARN(node->get_logger(), "object class_name: %s, target_frame: %s not match", obj.class_name.c_str(), tf.c_str());
}
}
RCLCPP_WARN(node->get_logger(), "object class_name not match target frames");
@@ -579,7 +590,7 @@ void CerebellumHooks::ConfigureVisionObjectRecognitionHooks(CerebellumNode * nod
pose_in_arm.pose.position.x, pose_in_arm.pose.position.y, pose_in_arm.pose.position.z,
pose_in_arm.pose.orientation.x, pose_in_arm.pose.orientation.y, pose_in_arm.pose.orientation.z, pose_in_arm.pose.orientation.w);
tf2::Vector3 target_pos(pose_in_arm.pose.position.x, pose_in_arm.pose.position.y, pose_in_arm.pose.position.z);
tf2::Vector3 target_pos(pose_in_arm.pose.position.x, pose_in_arm.pose.position.y, pose_in_arm.pose.position.z - node->grab_width_[1] * 0.1f);
tf2::Quaternion target_quat(pose_in_arm.pose.orientation.x, pose_in_arm.pose.orientation.y, pose_in_arm.pose.orientation.z, pose_in_arm.pose.orientation.w);
// std::vector<double> best_angles = brain::GraspPoseCalculator::find_optimal_grasp_angles(target_pos, node->arm_target_frame_, 3);
@@ -607,7 +618,7 @@ void CerebellumHooks::ConfigureVisionObjectRecognitionHooks(CerebellumNode * nod
for (double angle : best_angles) {
for (const auto& palm : palm_facings) {
auto result = brain::GraspPoseCalculator::calculate(target_pos,
target_quat, "side", angle, palm, 0.10, 0.185, node->arm_target_frame_);
target_quat, "side", angle, palm, 0.10, 0.20, node->arm_target_frame_);
geometry_msgs::msg::Pose pre_grasp_msg;
pre_grasp_msg.position.x = result.pre_grasp_pose.position.x();
@@ -661,7 +672,7 @@ void CerebellumHooks::ConfigureVisionObjectRecognitionHooks(CerebellumNode * nod
node->pre_grasp_poses_.push_back(pre_grasp_msg);
node->grasp_poses_.push_back(grasp_msg);
RCLCPP_INFO(node->get_logger(), "Generated Pose (Angle: %.1f, Palm: %s)", angle, palm.c_str());
RCLCPP_INFO(node->get_logger(), "Generated Pose (grasp_type: %s, Angle: %.1f, Palm: %s)", result.name.c_str(), angle, palm.c_str());
RCLCPP_INFO(node->get_logger(), " Pre: %.4f, %.4f, %.4f, %.4f, %.4f, %.4f, %.4f",
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());