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

test vision grasp

Browse Source
This commit is contained in:
NuoDaJia02
2025-12-16 10:02:15 +08:00
parent 180fbe4789
commit 0ced39b18d
5 changed files with 91 additions and 63 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -6,7 +6,7 @@
params: '{}
'
- name: gripper_open
- name: s1_gripper_open
params: 'loc: 0
speed: 20
@@ -16,14 +16,56 @@
mode: 2
'
- name: VisionObjectRecognition_H
- name: s2_VisionObjectRecognition_H
params: 'camera_position: right
'
- name: right_arm_vision_grasp
- name: s3_right_arm_vision_pre_grasp
params: 'body_id: 1
data_type: 0
data_type: 100
data_length: 0
command_id: 0
frame_time_stamp: 0
data_array: []
'
- name: s4_right_arm_vision_grasp
params: 'body_id: 1
data_type: 101
data_length: 0
command_id: 0
frame_time_stamp: 0
data_array: []
'
- name: s5_right_arm_vision_pre_grasp
params: 'body_id: 1
data_type: 100
data_length: 0
command_id: 0
frame_time_stamp: 0
data_array: []
'
- name: s6_right_arm_cam_take_photo
params: 'body_id: 1
data_type: 90
data_length: 0
@@ -35,7 +77,7 @@
'
- name: gripper_close
params: 'loc: 30
params: 'loc: 98
speed: 20

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

@@ -4,9 +4,12 @@
<!-- Retry vision + hand until GripperControl_H succeeds -->
<RetryUntilSuccessful name="retry_vision_hand" num_attempts="3">
<Sequence>
<!-- <GripperCmd_H name="gripper_open" /> -->
<VisionObjectRecognition_H name="VisionObjectRecognition_H" />
<!-- <Arm_H name="right_arm_vision_grasp" /> -->
<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" />
<!-- <GripperCmd_H name="gripper_close" /> -->
</Sequence>
</RetryUntilSuccessful>

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

@@ -49,6 +49,7 @@ public:
std::vector<double> left_arm_joint_angles_;
std::vector<double> right_arm_joint_angles_;
std::mutex joint_state_mutex_;
std::vector<double> right_arm_cam_joint_angles_;
private:
struct GoalExecutionContext { std::atomic<bool> cancel{false}; };

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

@@ -77,7 +77,27 @@ void CerebellumHooks::ConfigureArmHooks(CerebellumNode * node)
}
goal.data_length = USED_ARM_DOF;
}
} else {
} else if (goal.data_type == 90) { // 90: Set camera pose
goal.data_type = ARM_COMMAND_TYPE_ANGLE_DIRECT_MOVE;
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
goal.data_type = ARM_COMMAND_TYPE_POSE_DIRECT_MOVE;
auto pose = node->pre_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 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);
return goal;
}
@@ -526,44 +546,15 @@ void CerebellumHooks::ConfigureVisionObjectRecognitionHooks(CerebellumNode * nod
for (auto &tf: node->target_frames_) {
if (tf == obj.class_name) {
node->target_pose_[tf].header = header;
node->target_pose_[tf].header.frame_id = "RightLink6"; // Manually corrected frame
// Manual transform from Camera frame to RightLink6 frame (Rotate -90 deg around Z)
// Position: x_r = y_c, y_r = -x_c, z_r = z_c
node->target_pose_[tf].pose.position.x = obj.pose.position.y;
node->target_pose_[tf].pose.position.y = -obj.pose.position.x;
node->target_pose_[tf].pose.position.z = obj.pose.position.z;
// Orientation: q_r = q_rot(-90_Z) * q_c
// q_rot = (0, 0, -0.7071, 0.7071)
const double s = 0.70710678;
const double x = obj.pose.orientation.x;
const double y = obj.pose.orientation.y;
const double z = obj.pose.orientation.z;
const double w = obj.pose.orientation.w;
node->target_pose_[tf].pose.orientation.x = s * (x + y);
node->target_pose_[tf].pose.orientation.y = s * (y - x);
node->target_pose_[tf].pose.orientation.z = s * (z - w);
node->target_pose_[tf].pose.orientation.w = s * (w + z);
// node->target_pose_[tf].pose.position.x -= obj.grab_width / 2;
// node->target_pose_[tf].pose.position.z -= 0.175;
node->target_pose_[tf].header.frame_id = "RightLink6";
node->target_pose_[tf].pose = obj.pose;
node->grab_width_ = obj.grab_width;
// RCLCPP_INFO(node->get_logger(), "target_frame_: %s, grab_width_: %f, updated target_pose",
// tf.c_str(), node->grab_width_);
const auto & p = obj.pose;
RCLCPP_INFO(node->get_logger(), "target_frame: %s, grab_width: %f, t=%.4f, target_pose: %.4f, %.4f, %.4f, %.4f, %.4f, %.4f, %.4f",
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::Time(node->target_pose_[tf].header.stamp).seconds(),
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] not match target_frame_[%s]",
// obj.class_name.c_str(), tf.c_str());
// return false;
// }
}
RCLCPP_WARN(node->get_logger(), "object class_name not match target frames");
return false;
@@ -581,8 +572,6 @@ void CerebellumHooks::ConfigureVisionObjectRecognitionHooks(CerebellumNode * nod
skill_name.c_str(), pose_in_arm.header.frame_id.c_str());
return false;
}
pose_in_arm.pose.position.y -= 0.175;
{
RCLCPP_INFO(node->get_logger(), "[%s] target pose transformed to %s frame", skill_name.c_str(), node->arm_target_frame_.c_str());
@@ -593,23 +582,16 @@ void CerebellumHooks::ConfigureVisionObjectRecognitionHooks(CerebellumNode * nod
tf2::Vector3 target_pos(pose_in_arm.pose.position.x, pose_in_arm.pose.position.y, pose_in_arm.pose.position.z);
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);
if (best_angles.empty()) {
RCLCPP_WARN(node->get_logger(), "[%s]no valid grasp angles found", skill_name.c_str());
return false;
}
// std::vector<double> best_angles = brain::GraspPoseCalculator::find_optimal_grasp_angles(target_pos, node->arm_target_frame_, 3);
// if (best_angles.empty()) {
// RCLCPP_WARN(node->get_logger(), "[%s]no valid grasp angles found", skill_name.c_str());
// return false;
// }
// best_angles.clear();
best_angles.push_back(-45);
std::vector<double> best_angles;
best_angles.push_back(0);
best_angles.push_back(-60);
best_angles.push_back(-75);
best_angles.push_back(-90);
best_angles.push_back(20);
best_angles.push_back(45);
best_angles.push_back(60);
{
std::vector<std::string> palm_facings = {"down"}; //{"up", "down"};
std::vector<double> current_joint_angles;
@@ -625,7 +607,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.06, 0.0, node->arm_target_frame_);
target_quat, "side", angle, palm, 0.10, 0.185, node->arm_target_frame_);
geometry_msgs::msg::Pose pre_grasp_msg;
pre_grasp_msg.position.x = result.pre_grasp_pose.position.x();
@@ -761,6 +743,10 @@ void CerebellumHooks::ConfigureVisionObjectRecognitionHooks(CerebellumNode * nod
skill_name.c_str(), node->pre_grasp_poses_.size(), node->grasp_poses_.size());
return false;
}
node->right_arm_cam_joint_angles_.clear();
node->right_arm_cam_joint_angles_ = node->right_arm_joint_angles_;
return true;
};
node->skill_manager_->configure_service_hooks<VisionSrv>("VisionObjectRecognition", std::move(hooks));

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

@@ -327,6 +327,7 @@ bool CerebellumNode::TransformPoseToArmFrame(
source, arm_target_frame_, tf2::durationFromSec(arm_transform_timeout_sec_));
// ===================
#ifdef TF_DEBUG_LOGGING
RCLCPP_INFO(this->get_logger(),
"TF Debug: Source [%s] pos(%.4f, %.4f, %.4f) quat(%.4f, %.4f, %.4f, %.4f) t=%.4f",
source.header.frame_id.c_str(),
@@ -339,6 +340,7 @@ bool CerebellumNode::TransformPoseToArmFrame(
transformed.header.frame_id.c_str(),
transformed.pose.position.x, transformed.pose.position.y, transformed.pose.position.z,
transformed.pose.orientation.x, transformed.pose.orientation.y, transformed.pose.orientation.z, transformed.pose.orientation.w);
#endif
// ===================
RCLCPP_INFO(
@@ -637,12 +639,6 @@ bool CerebellumNode::ExecuteActionSkill(
}
return RunActionSkillWithProgress(skill, topic, effective_timeout,
instance_name, timeout_ms_override, index, total_skills, goal_handle, detail);
// return (skill == "Arm") ?
// ExecuteBilateralArmAction(skill, topic, effective_timeout,
// instance_name, timeout_ms_override, index, total_skills, goal_handle, detail) :
// RunActionSkillWithProgress(skill, topic, effective_timeout,
// instance_name, timeout_ms_override, index, total_skills, goal_handle, detail);
}
/**