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

fix cal pose

Browse Source
This commit is contained in:
NuoDaJia02
2025-12-14 18:27:58 +08:00
parent afc4d983c8
commit 180fbe4789
6 changed files with 103 additions and 60 deletions
Download Patch File Download Diff File Expand all files Collapse all files

2
src/brain/CMakeLists.txt
View File

@@ -80,6 +80,8 @@ install(TARGETS brain_node
install(DIRECTORY config/
DESTINATION share/${PROJECT_NAME}/config)
set (BUILD_TESTING OFF)
if(BUILD_TESTING)
if(NOT DEFINED ENV{SKIP_LINT})
find_package(ament_lint_auto REQUIRED)

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

@@ -82,11 +82,11 @@ GraspResult GraspPoseCalculator::calculate(
std::string name_suffix;
if (grasp_type == "side") {
tf2::Vector3 V_horiz_sb(0, V_sb.y(), V_sb.z());
// tf2::Vector3 V_horiz_sb(0, V_sb.y(), V_sb.z());
double base_angle = 0.0;
if (V_horiz_sb.length() >= 1e-3) {
base_angle = std::atan2(V_horiz_sb.z(), V_horiz_sb.y());
}
// if (V_horiz_sb.length() >= 1e-3) {
// base_angle = std::atan2(V_horiz_sb.z(), V_horiz_sb.y());
// }
double rad = angle_deg * M_PI / 180.0;
double angle = base_angle + rad;
double dy = std::cos(angle);
@@ -112,6 +112,42 @@ GraspResult GraspPoseCalculator::calculate(
GraspResult result = create_grasp_matrix(target_pos, approach_vec, palm_normal, safety_dist, finger_length);
result.name = name_suffix;
// Check IK and adjust position if needed (Tolerance +/- 2cm)
std::vector<double> q_out(7, 0.0);
// Try original position first
if (rm_arm_inverse_kinematics(result.grasp_pose.position, result.grasp_pose.orientation, q_out) != 0) {
// Search in +/- 2cm box
bool found = false;
double tolerance = 0.02;
double step = 0.005; // 5mm step
for (double dx = -tolerance; dx <= tolerance + 1e-5; dx += step) {
for (double dy = -tolerance; dy <= tolerance + 1e-5; dy += step) {
for (double dz = -tolerance; dz <= tolerance + 1e-5; dz += step) {
if (std::abs(dx) < 1e-5 && std::abs(dy) < 1e-5 && std::abs(dz) < 1e-5) continue;
tf2::Vector3 new_target = target_pos + tf2::Vector3(dx, dy, dz);
GraspResult new_result = create_grasp_matrix(new_target, approach_vec, palm_normal, safety_dist, finger_length);
new_result.name = name_suffix;
if (rm_arm_inverse_kinematics(new_result.grasp_pose.position, new_result.grasp_pose.orientation, q_out) == 0) {
result = new_result;
found = true;
RCLCPP_INFO(rclcpp::get_logger("GraspPoseCalculator"),
"Adjusted grasp target by [%.3f, %.3f, %.3f] to satisfy IK.", dx, dy, dz);
break;
}
}
if (found) break;
}
if (found) break;
}
if (!found) {
RCLCPP_WARN(rclcpp::get_logger("GraspPoseCalculator"), "Could not find valid IK within tolerance.");
}
}
return result;
}
@@ -122,7 +158,8 @@ double GraspPoseCalculator::evaluate_grasp_quality(
) {
double score = 0.0;
bool is_right = (arm.find("right") != std::string::npos);
bool is_outward = (is_right && angle_deg < 0) || (!is_right && angle_deg > 0);
// In this frame (Z is Right), angle > 0 moves towards Z (Right/Outward)
bool is_outward = (is_right && angle_deg > 0) || (!is_right && angle_deg < 0);
if (is_outward) {
score += 50.0;
@@ -240,7 +277,8 @@ int GraspPoseCalculator::rm_arm_inverse_kinematics(
int result = rm_algo_inverse_kinematics(NULL, ik_params, joints);
if (result != 0) {
RCLCPP_ERROR(rclcpp::get_logger("GraspPoseCalculator"), "Inverse kinematics failed with error code %d", result);
// Downgrade to DEBUG to avoid spamming during search
RCLCPP_DEBUG(rclcpp::get_logger("GraspPoseCalculator"), "Inverse kinematics failed with error code %d", result);
return ARM_AIM_CANNOT_REACH;
}
for (int i = 0; i < USED_ARM_DOF; i++) {

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

@@ -526,17 +526,36 @@ 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";
node->target_pose_[tf].pose = obj.pose;
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].pose.position.z -= 0.175;
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, target_pose: %.4f, %.4f, %.4f, %.4f, %.4f, %.4f, %.4f",
RCLCPP_INFO(node->get_logger(), "target_frame: %s, grab_width: %f, t=%.4f, 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;
}
@@ -562,6 +581,8 @@ 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());
@@ -584,6 +605,10 @@ void CerebellumHooks::ConfigureVisionObjectRecognitionHooks(CerebellumNode * nod
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"};

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

@@ -325,6 +325,22 @@ bool CerebellumNode::TransformPoseToArmFrame(
try {
transformed = tf_buffer_->transform(
source, arm_target_frame_, tf2::durationFromSec(arm_transform_timeout_sec_));
// ===================
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(),
source.pose.position.x, source.pose.position.y, source.pose.position.z,
source.pose.orientation.x, source.pose.orientation.y, source.pose.orientation.z, source.pose.orientation.w,
rclcpp::Time(source.header.stamp).seconds());
RCLCPP_INFO(this->get_logger(),
"TF Debug: Result [%s] pos(%.4f, %.4f, %.4f) quat(%.4f, %.4f, %.4f, %.4f)",
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);
// ===================
RCLCPP_INFO(
this->get_logger(), "Target pose transformed from %s to %s",
source.header.frame_id.c_str(), arm_target_frame_.c_str());

62
src/modules/vision/vision_object_recg/src/vision_object_recg_node.cpp
View File

@@ -44,64 +44,26 @@ private:
// Fake two types of objects (example: cup / bottle), each type gives 1~2 Poses
interfaces::msg::PoseClassAndID obj1;
obj1.class_name = "bottlex";
obj1.class_name = "bottle";
obj1.class_id = 1;
//{-0.9758, 0, 0.42, -0.5, 0.5, 0.5, -0.5};
geometry_msgs::msg::Pose pose1; // 基本位姿
// pose1.position.x = 0.9758;
// pose1.position.y = 0;
// pose1.position.z = 0.42;
// pose1.orientation.x = -0.5;
// pose1.orientation.y = 0.5;
// pose1.orientation.z = 0.5;
// pose1.orientation.w = -0.5;
//geometry_msgs.msg.Pose(position=geometry_msgs.msg.Point(x=-0.2851924786746129, y=-0.056353812689333635, z=1.073772448259523),
//orientation=geometry_msgs.msg.Quaternion(x=-0.16713377464857188, y=-0.42460237568763715, z=-0.26706232441180955, w=0.8487972895879773))
// pose1.position.x = -0.2851924786746129;
// pose1.position.y = -0.056353812689333635;
// pose1.position.z = 1.073772448259523;
// pose1.orientation.x = -0.16713377464857188;
// pose1.orientation.y = -0.42460237568763715;
// pose1.orientation.z = -0.26706232441180955;
// pose1.orientation.w = 0.8487972895879773;
//222.853, 514.124, 261.742
//-0.65115316, 0.05180144, -0.19539139, 0.73153153
//-0.63142093, 0.18186004, -0.12407289, 0.74353241
pose1.position.x = 222.853;
pose1.position.y = 514.124;
pose1.position.z = 261.742;
pose1.orientation.x = -0.65115316;
pose1.orientation.y = 0.05180144;
pose1.orientation.z = -0.19539139;
pose1.orientation.w = 0.73153153;
// obj1.pose_list.push_back(pose1);
// 第二个同类实例(示例)
geometry_msgs::msg::Pose pose1b = pose1;
pose1b.position.y += 0.05;
// obj1.pose_list.push_back(pose1b);
interfaces::msg::PoseClassAndID obj2;
obj2.class_name = "cup";
obj2.class_id = 2;
geometry_msgs::msg::Pose pose2;
pose2.position.x = 0.5;
pose2.position.y = -0.2;
pose2.position.z = 0.78;
pose2.orientation.w = 1.0;
// obj2.pose_list.push_back(pose2);
//target_frame: bottle, grab_width: 0.046393, target_pose: -0.0067, -0.0278, 0.2976, -0.1379, -0.0965, 0.5650, 0.8078
pose1.position.x = -0.0067;
pose1.position.y = -0.0278;
pose1.position.z = 0.2976;
pose1.orientation.x = -0.1379;
pose1.orientation.y = -0.0965;
pose1.orientation.z = 0.5650;
pose1.orientation.w = 0.8078;
obj1.pose = pose1;
obj1.grab_width = 0.046393;
res->objects.clear();
res->objects.emplace_back(obj1);
res->objects.emplace_back(obj2);
std::ostringstream oss;
// oss << "Recognized " << res->objects.size() << " object classes (total instances: "
// << (obj1.pose_list.size() + obj2.pose_list.size()) << ")";
oss << "Recognized " << res->objects.size() << " object classes (total instances: " << res->objects.size() << ")";
res->info = oss.str();
} else {
res->success = false;

BIN
src/thirdparty/rm_arm/lib/libapi_cpp.so vendored
View File

Binary file not shown.