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

modify parameters

Browse Source
This commit is contained in:
NuoDaJia02
2026-01-13 16:59:58 +08:00
parent 2d4a1edb10
commit a36903f467
3 changed files with 22 additions and 22 deletions
Download Patch File Download Diff File Expand all files Collapse all files

8
src/brain/config/bt_vision_grasp_dual_arm.params.yaml
View File

@@ -25,7 +25,7 @@
frame_time_stamp: 0
data_array: [0, 0, 0, 0, 0, 0, 95, -25, -90, 0, -68, 90]
data_array: [0, 0, 0, 0, 0, 0, 95, -22, -90, 0, -68, 90]
'
- name: s1_left_arm_initial
@@ -39,7 +39,7 @@
frame_time_stamp: 0
data_array: [85, 25, 90, 0, 68, 90, 0, 0, 0, 0, 0, 0]
data_array: [85, 22, 90, 0, 68, 90, 0, 0, 0, 0, 0, 0]
'
- name: s1_right_arm_pre_cam_take_photo
@@ -53,7 +53,7 @@
frame_time_stamp: 0
data_array: [0, 0, 0, 0, 0, 0, 60, -20, -80, 0, -80, 60]
data_array: [0, 0, 0, 0, 0, 0, 60, -22, -90, 0, -68, 90]
'
- name: s1_left_arm_pre_cam_take_photo
@@ -67,7 +67,7 @@
frame_time_stamp: 0
data_array: [120, 20, 80, 0, 80, 120, 0, 0, 0, 0, 0, 0]
data_array: [120, 22, 90, 0, 68, 90, 0, 0, 0, 0, 0, 0]
'
- name: s2_right_arm_cam_take_photo

11
src/brain/config/bt_vision_grasp_dual_arm_demo1.params.yaml
View File

@@ -25,7 +25,7 @@
frame_time_stamp: 0
data_array: [0, 0, 0, 0, 0, 0, 95, -25, -90, 0, -68, 90]
data_array: [0, 0, 0, 0, 0, 0, 95, -22, -90, 0, -68, 90]
'
- name: s1_left_arm_initial
@@ -39,7 +39,7 @@
frame_time_stamp: 0
data_array: [85, 25, 90, 0, 68, 90, 0, 0, 0, 0, 0, 0]
data_array: [85, 22, 90, 0, 68, 90, 0, 0, 0, 0, 0, 0]
'
- name: s1_right_arm_pre_cam_take_photo
@@ -53,7 +53,7 @@
frame_time_stamp: 0
data_array: [0, 0, 0, 0, 0, 0, 60, -20, -80, 0, -80, 60]
data_array: [0, 0, 0, 0, 0, 0, 60, -22, -90, 0, -68, 90]
'
- name: s1_left_arm_pre_cam_take_photo
@@ -67,7 +67,7 @@
frame_time_stamp: 0
data_array: [120, 20, 80, 0, 80, 120, 0, 0, 0, 0, 0, 0]
data_array: [120, 22, 90, 0, 68, 90, 0, 0, 0, 0, 0, 0]
'
- name: s2_right_arm_cam_take_photo
@@ -86,6 +86,7 @@
'
- name: s2_left_arm_cam_take_photo
params: &left_arm_cam_take_photo 'body_id: 0
data_type: 100
data_length: 0
@@ -172,7 +173,7 @@
torque: 100
mode: 1
mode: 2
'
- name: s4_left_hand_gripper_close

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

@@ -79,7 +79,7 @@ void CerebellumHooks::ConfigureArmHooks(CerebellumNode * node)
}
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 <= 120) {
geometry_msgs::msg::Pose pose;
std::vector<double> angle{360.0, 360.0, 360.0, 360.0, 360.0, 360.0}; //invalid angles
if (goal.data_type == 100) { //take photo
@@ -657,15 +657,14 @@ void CerebellumHooks::ConfigureVisionObjectRecognitionHooks(CerebellumNode * nod
if (target_pose.pose.position.z > 0.10f) { //right arm
target_arm = "right_arm";
target_pose.pose.position.x += 0.12f;
target_pose.pose.position.x += 0.10f;
target_pose.pose.position.y = 0.32f;
// target_pose.pose.position.z += 0.03f;
target_pose.pose.orientation.x = -0.7071;
target_pose.pose.orientation.y = 0.0000;
target_pose.pose.orientation.z = -0.0000;
target_pose.pose.orientation.w = 0.7071;
} else if (target_pose.pose.position.z < -0.15f) { //left arm
} else if (target_pose.pose.position.z < -0.10f) { //left arm
//transfer to left arm base
target_arm = "left_arm";
geometry_msgs::msg::PoseStamped left_arm_pose;
@@ -678,7 +677,6 @@ void CerebellumHooks::ConfigureVisionObjectRecognitionHooks(CerebellumNode * nod
target_pose.pose = left_arm_pose.pose;
target_pose.pose.position.x += 0.10f;
target_pose.pose.position.y = -0.32f;
// target_pose.pose.position.z += 0.03f;
target_pose.pose.orientation.x = 0.7071;
target_pose.pose.orientation.y = 0.0000;
target_pose.pose.orientation.z = -0.0000;
@@ -794,13 +792,13 @@ void CerebellumHooks::ConfigureVisionObjectRecognitionHooks(CerebellumNode * nod
result.grasp_pose.orientation.x(), result.grasp_pose.orientation.y(), result.grasp_pose.orientation.z(), result.grasp_pose.orientation.w());
#endif
// if (node->camera_position_ == "right") {
// result.grasp_pose.position.setZ(result.grasp_pose.position.z() - 0.015); //补偿抓取的偏差
// result.pre_grasp_pose.position.setZ(result.pre_grasp_pose.position.z() - 0.015); //补偿抓取的偏差
// } else if (node->camera_position_ == "left") {
// result.grasp_pose.position.setZ(result.grasp_pose.position.z() + 0.01); //补偿抓取的偏差
// result.pre_grasp_pose.position.setZ(result.pre_grasp_pose.position.z() + 0.01); //补偿抓取的偏差
// }
if (node->camera_position_ == "right") {
result.grasp_pose.position.setZ(result.grasp_pose.position.z() + 0.02); //补偿抓取的偏差
result.pre_grasp_pose.position.setZ(result.pre_grasp_pose.position.z() + 0.02); //补偿抓取的偏差
} else if (node->camera_position_ == "left") {
result.grasp_pose.position.setZ(result.grasp_pose.position.z() + 0.01); //补偿抓取的偏差
result.pre_grasp_pose.position.setZ(result.pre_grasp_pose.position.z() + 0.01); //补偿抓取的偏差
}
//pre grasp
std::vector<double> pre_grasp_joint_angles = current_joint_angles;
@@ -821,7 +819,8 @@ void CerebellumHooks::ConfigureVisionObjectRecognitionHooks(CerebellumNode * nod
}
//take object
auto take_object_pose = result.grasp_pose;
take_object_pose.position.setX(take_object_pose.position.x() - 0.10); //向上拿起物体
// take_object_pose.position.setX(take_object_pose.position.x() - 0.10); //向上拿起物体
take_object_pose.position.setX(-0.05f); //向上拿起物体
std::vector<double> take_object_joint_angles = current_joint_angles;
if (brain::GraspPoseCalculator::rm_arm_inverse_kinematics(take_object_pose.position,
take_object_pose.orientation, take_object_joint_angles) != 0) {