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

test left arm grasp

Browse Source
This commit is contained in:
NuoDaJia02
2026-01-09 16:57:33 +08:00
parent 79187e6645
commit 2348200dc7
5 changed files with 54 additions and 23 deletions
Download Patch File Download Diff File Expand all files Collapse all files

1
.gitignore vendored
View File

@@ -1,6 +1,7 @@
build/
install/
log/
logs/
.vscode/
.venv/

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

@@ -70,7 +70,8 @@ GraspResult GraspPoseCalculator::calculate(
const std::string& arm
) {
(void)target_quat;
(void)arm;
bool is_left = (arm.find("left") != std::string::npos);
tf2::Vector3 V_world_up(-1.0, 0.0, 0.0);
tf2::Vector3 V_world_down = -V_world_up;
@@ -96,6 +97,9 @@ GraspResult GraspPoseCalculator::calculate(
double angle = base_angle + rad;
double dy = std::cos(angle);
double dz = std::sin(angle);
if (is_left) {
dy = -dy;
}
approach_vec = tf2::Vector3(0, dy, dz); // Horizontal approach
if (palm_facing == "up") {

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

@@ -677,7 +677,7 @@ void CerebellumHooks::ConfigureVisionObjectRecognitionHooks(CerebellumNode * nod
}
target_pose.pose = left_arm_pose.pose;
target_pose.pose.position.x += 0.08f;
target_pose.pose.position.y = 0.30f;
target_pose.pose.position.y = -0.30f;
target_pose.pose.position.z += 0.03f;
target_pose.pose.orientation.x = 0.7071;
target_pose.pose.orientation.y = 0.0000;
@@ -691,11 +691,11 @@ 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);
if (pose.position.x < 0.1 || pose.position.y < 0.1 || pose.position.z < 0.1 ||
pose.position.x > 0.8 || pose.position.y > 0.8 || pose.position.z > 1.0) {
RCLCPP_ERROR(node->get_logger(), "invalid right_arm pose value, cal grasp pose failed");
return false;
}
// if (pose.position.x < 0.1 || pose.position.y < 0.1 || pose.position.z < 0.1 ||
// pose.position.x > 0.8 || pose.position.y > 0.8 || pose.position.z > 1.0) {
// RCLCPP_ERROR(node->get_logger(), "invalid %s pose value, cal grasp pose failed", target_arm.c_str());
// return false;
// }
std::vector<double> joint_angles;
{
@@ -710,15 +710,15 @@ void CerebellumHooks::ConfigureVisionObjectRecognitionHooks(CerebellumNode * nod
tf2::Vector3 target_pos = {pose.position.x, pose.position.y, pose.position.z};
tf2::Quaternion target_quat = {pose.orientation.x, pose.orientation.y, pose.orientation.z, pose.orientation.w};
if (brain::GraspPoseCalculator::rm_arm_inverse_kinematics(target_pos, target_quat, joint_angles) != 0) {
RCLCPP_ERROR(node->get_logger(), "right_hand take_photo_pose rm arm inverse kinematics failed.");
RCLCPP_ERROR(node->get_logger(), "%s take_photo_pose rm arm inverse kinematics failed.", target_arm.c_str());
return false;
}
#ifdef DEBUG_GRASP_POSE_CALCULATION
if (joint_angles.size() >= 6) {
RCLCPP_INFO(node->get_logger(), "right_hand take_photo joint angles: [%.4f, %.4f, %.4f, %.4f, %.4f, %.4f]",
joint_angles[0], joint_angles[1], joint_angles[2], joint_angles[3], joint_angles[4], joint_angles[5]);
RCLCPP_INFO(node->get_logger(), "%s take_photo joint angles: [%.4f, %.4f, %.4f, %.4f, %.4f, %.4f]",
target_arm.c_str(), joint_angles[0], joint_angles[1], joint_angles[2], joint_angles[3], joint_angles[4], joint_angles[5]);
} else {
RCLCPP_ERROR(node->get_logger(), "right_hand take_photo joint angles size invalid: %ld", joint_angles.size());
RCLCPP_ERROR(node->get_logger(), "%s take_photo joint angles size invalid: %ld", target_arm.c_str(), joint_angles.size());
}
#endif
@@ -740,8 +740,10 @@ void CerebellumHooks::ConfigureVisionObjectRecognitionHooks(CerebellumNode * nod
std::lock_guard<std::mutex> lk(node->joint_state_mutex_);
if (node->camera_position_ == "right") {
current_joint_angles = node->right_arm_joint_angles_;
node->arm_target_frame_ = "base_link_rightarm";
} else if (node->camera_position_ == "left") {
current_joint_angles = node->left_arm_joint_angles_;
node->arm_target_frame_ = "base_link_leftarm";
}
}
@@ -975,11 +977,11 @@ void CerebellumHooks::ConfigureVisionObjectRecognitionHooks(CerebellumNode * nod
if (node->camera_position_ == "top") {
if (node->right_arm_take_photo_poses_.empty() && node->left_arm_take_photo_poses_.empty()) {
RCLCPP_WARN(node->get_logger(), "[%s] no valid right hand take photo poses generated", skill_name.c_str());
RCLCPP_WARN(node->get_logger(), "[%s] no valid hand take photo poses generated", skill_name.c_str());
return false;
}
if (node->right_arm_take_photo_angles_.empty() && node->left_arm_take_photo_angles_.empty()) {
RCLCPP_WARN(node->get_logger(), "[%s] no valid right hand take photo angles generated", skill_name.c_str());
RCLCPP_WARN(node->get_logger(), "[%s] no valid hand take photo angles generated", skill_name.c_str());
return false;
}
} else if (node->camera_position_ == "right") {

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

@@ -323,6 +323,8 @@ void CerebellumNode::SetupExecuteBtServer()
action_registry_->register_action_server<EBA>(brain::kExecuteBtActionName, h);
}
#define TF_DEBUG_LOGGING 1
bool CerebellumNode::TransformPoseToArmFrame(
const geometry_msgs::msg::PoseStamped & source, const std::string & arm_target_frame,
geometry_msgs::msg::PoseStamped & transformed) const

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

@@ -44,20 +44,42 @@ private:
// Fake two types of objects (example: cup / bottle), each type gives 1~2 Poses
interfaces::msg::PoseClassAndID obj1;
obj1.class_name = "bottle";
obj1.class_name = "medicine_box";
obj1.class_id = 1;
geometry_msgs::msg::Pose pose1; // 基本位姿
//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;
if (req->camera_position == "top") {
//0.1647, 0.7436, -0.2081, 0.4809, 0.7398, 0.4006, 0.2469
pose1.position.x = 0.1647;
pose1.position.y = 0.7436;
pose1.position.z = -0.2081;
pose1.orientation.x = 0.4809;
pose1.orientation.y = 0.7398;
pose1.orientation.z = 0.4006;
pose1.orientation.w = 0.2469;
} else if (req->camera_position == "left") {
//0.2320, -0.6137, 0.2141, 0.3670, 0.7705, -0.4314, 0.2924
pose1.position.x = 0.2320;
pose1.position.y = 0.6137;
pose1.position.z = 0.2141;
pose1.orientation.x = -0.3670;
pose1.orientation.y = 0.7705;
pose1.orientation.z = 0.4314;
pose1.orientation.w = 0.2924;
} else if (req->camera_position == "right") {
//-0.0165, 0.0246, 0.4166, 0.2939, -0.2219, -0.6882, 0.6251
pose1.position.x = -0.0165;
pose1.position.y = 0.0246;
pose1.position.z = 0.4166;
pose1.orientation.x = 0.2939;
pose1.orientation.y = -0.2219;
pose1.orientation.z = -0.6882;
pose1.orientation.w = 0.6251;
}
obj1.pose = pose1;
obj1.grab_width = 0.046393;
obj1.grab_width = {0.046393, 0.065393, 0.046393}; // Example grab widths
res->objects.clear();
res->objects.emplace_back(obj1);