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

optimize code

Browse Source
This commit is contained in:
NuoDaJia02
2025-12-30 13:52:49 +08:00
parent 56ec283a44
commit b063044249
2 changed files with 5 additions and 6 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -6,7 +6,6 @@
<Arm_H name="s1_right_arm_pre_cam_take_photo" />
<Parallel name="parallel_action_1">
<MoveWheel_H name="s1_wheel_move_to_origin_pickup_position" />
<!-- <MoveWaist_H name="s1_move_waist_turn_right_90" /> -->
<GripperCmd_H name="s1_gripper_open" />
<Arm_H name="s2_right_arm_pre_cam_take_photo" />
</Parallel>
@@ -26,8 +25,8 @@
<Arm_H name="s5_right_arm_grasp_box" />
<Parallel name="parallel_action_2">
<MoveWaist_H name="s6_move_waist_turn_left_90" />
<MoveWheel_H name="s6_wheel_move_to_putdown_position" />
<MoveWaist_H name="s6_move_waist_turn_left_90" />
</Parallel>
<Arm_H name="s7_right_arm_grasp_box" />
<Arm_H name="s7_right_arm_put_down_box" />

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

@@ -92,21 +92,21 @@ void CerebellumHooks::ConfigureArmHooks(CerebellumNode * node)
pose = node->pre_grasp_poses_[0];
}
if (!node->pre_grasp_angles_.empty()) {
angle = node->pre_grasp_angles_[0];
angle = node->pre_grasp_angles_[node->pre_grasp_angles_.size()-1];
}
} else if (goal.data_type == 102) { // grasp
if (!node->grasp_poses_.empty()) {
pose = node->grasp_poses_[0];
}
if (!node->grasp_angles_.empty()) {
angle = node->grasp_angles_[0];
angle = node->grasp_angles_[node->grasp_angles_.size()-1];
}
} else if (goal.data_type == 110) { //take object
if (!node->right_hand_take_object_pose_.empty()) {
pose = node->right_hand_take_object_pose_[0];
}
if (!node->right_hand_take_object_angles_.empty()) {
angle = node->right_hand_take_object_angles_[0];
angle = node->right_hand_take_object_angles_[node->right_hand_take_object_angles_.size()-1];
}
} else {
RCLCPP_ERROR(node->get_logger(), "ARM make goal error: invalid data_type");
@@ -725,7 +725,7 @@ 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.06); //向上拿起物体
take_object_pose.position.setX(take_object_pose.position.x() - 0.10); //向上拿起物体
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) {