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

support arm movej and movep

Browse Source
This commit is contained in:
david
2025-10-28 10:59:06 +08:00
parent b2a7bada15
commit e8ecc73b69
3 changed files with 52 additions and 40 deletions
Download Patch File Download Diff File Expand all files Collapse all files

30
src/brain/config/bt_carry_boxes_sch1.params.yaml
View File

@@ -15,15 +15,15 @@
- name: s2_arm_stretch_out
params: 'body_id: 0
data_type: 0
data_type: 2
data_length: 14
data_length: 12
command_id: 0
frame_time_stamp: 0
data_array: [-0.222853, -0.514124, 0.261742, -0.18186004, -0.63142093, 0.74353241, 0.12407289, 0.086304, 0.471622, 0.033076, -0.76833478, -0.08777985, -0.21307887, 0.59712123]
data_array: [-76.15, -59.43, -39.243, 0.134, -77.463, -168.251, -102.251, 55.548, 33.71, -1.496, 92.311, -9.148]
'
- name: s3_hand_pickup
@@ -35,15 +35,15 @@
- name: s4_arm_lift
params: 'body_id: 0
data_type: 0
data_type: 2
data_length: 14
data_length: 12
command_id: 0
frame_time_stamp: 0
data_array: [-0.198632, -0.513333, 0.072526, -0.13862565, -0.71159701, 0.67387035, 0.14251799, 0.253501, 0.476683, 0.265433, -0.51013004, -0.02328561, -0.48576245, 0.70940817]
data_array: [-76.15, -59.43, -39.243, 0.134, -77.463, -168.251, -102.251, 55.548, 33.71, -1.496, 92.311, -9.148]
'
- name: s5_waist_bend_up
@@ -77,15 +77,15 @@
- name: s10_arm_stretch_out
params: 'body_id: 0
data_type: 0
data_type: 2
data_length: 14
data_length: 12
command_id: 0
frame_time_stamp: 0
data_array: [-0.222853, -0.514124, 0.261742, -0.18186004, -0.63142093, 0.74353241, 0.12407289, 0.325695, 0.447487, 0.108462, -0.65995416, 0.33866696, -0.16590247, 0.64980117]
data_array: [-76.15, -59.43, -39.243, 0.134, -77.463, -168.251, -102.251, 55.548, 33.71, -1.496, 92.311, -9.148]
'
- name: s11_hand_release
@@ -97,15 +97,15 @@
- name: s12_arm_move_to_snapshot_pose
params: 'body_id: 0
data_type: 0
data_type: 2
data_length: 14
data_length: 12
command_id: 0
frame_time_stamp: 0
data_array: [-0.179387, -0.341708, -0.099286, -0.12851699, -0.88140507, 0.41951188, 0.17498077, 0.128250, 0.554955, 0.096719, -0.66087188, 0.14284399, -0.04440424, 0.73544015]
data_array: [-76.15, -59.43, -39.243, 0.134, -77.463, -168.251, -102.251, 55.548, 33.71, -1.496, 92.311, -9.148]
'
- name: s13_camera_take_photo
@@ -123,14 +123,14 @@
- name: s15_arm_retract
params: 'body_id: 0
data_type: 0
data_type: 2
data_length: 14
data_length: 12
command_id: 0
frame_time_stamp: 0
data_array: [-0.123562, -0.555266, 0.062529, -0.02276690, -0.75662638, 0.63753626, 0.14333775, 0.082317, 0.557912, 0.123469, -0.63065177, -0.03018818, -0.18611339, 0.75281394]
data_array: [-76.15, -59.43, -39.243, 0.134, -77.463, -168.251, -102.251, 55.548, 33.71, -1.496, 92.311, -9.148]
'

24
src/brain/config/bt_carry_boxes_sch2.params.yaml
View File

@@ -15,15 +15,15 @@
- name: s2_arm_stretch_out
params: 'body_id: 0
data_type: 0
data_type: 2
data_length: 14
data_length: 12
command_id: 0
frame_time_stamp: 0
data_array: [-0.222853, -0.514124, 0.261742, -0.18186004, -0.63142093, 0.74353241, 0.12407289, 0.086304, 0.471622, 0.033076, -0.76833478, -0.08777985, -0.21307887, 0.59712123]
data_array: [-76.15, -59.43, -39.243, 0.134, -77.463, -168.251, -102.251, 55.548, 33.71, -1.496, 92.311, -9.148]
'
- name: s3_hand_pickup
@@ -35,15 +35,15 @@
- name: s4_arm_lift
params: 'body_id: 0
data_type: 0
data_type: 2
data_length: 14
data_length: 12
command_id: 0
frame_time_stamp: 0
data_array: [-0.198632, -0.513333, 0.072526, -0.13862565, -0.71159701, 0.67387035, 0.14251799, 0.253501, 0.476683, 0.265433, -0.51013004, -0.02328561, -0.48576245, 0.70940817]
data_array: [-76.15, -59.43, -39.243, 0.134, -77.463, -168.251, -102.251, 55.548, 33.71, -1.496, 92.311, -9.148]
'
- name: s5_waist_bend_up
@@ -77,15 +77,15 @@
- name: s10_arm_stretch_out
params: 'body_id: 0
data_type: 0
data_type: 2
data_length: 14
data_length: 12
command_id: 0
frame_time_stamp: 0
data_array: [-0.222853, -0.514124, 0.261742, -0.18186004, -0.63142093, 0.74353241, 0.12407289, 0.325695, 0.447487, 0.108462, -0.65995416, 0.33866696, -0.16590247, 0.64980117]
data_array: [-76.15, -59.43, -39.243, 0.134, -77.463, -168.251, -102.251, 55.548, 33.71, -1.496, 92.311, -9.148]
'
- name: s11_hand_release
@@ -97,14 +97,14 @@
- name: s15_arm_retract
params: 'body_id: 0
data_type: 0
data_type: 2
data_length: 14
data_length: 12
command_id: 0
frame_time_stamp: 0
data_array: [-0.123562, -0.555266, 0.062529, -0.02276690, -0.75662638, 0.63753626, 0.14333775, 0.082317, 0.557912, 0.123469, -0.63065177, -0.03018818, -0.18611339, 0.75281394]
data_array: [-76.15, -59.43, -39.243, 0.134, -77.463, -168.251, -102.251, 55.548, 33.71, -1.496, 92.311, -9.148]
'

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

@@ -128,22 +128,34 @@ void CerebellumNode::ConfigureArmHooks()
goal.body_id = (tls_arm_body_id == LEFT_ARM ||
tls_arm_body_id == RIGHT_ARM) ? tls_arm_body_id : LEFT_ARM;
goal.data_type = ARM_COMMAND_TYPE_POSE_DIRECT_MOVE;
goal.command_id = ++command_id_;
if (goal.data_length == 14 && goal.data_array.size() == 14) {
if (goal.body_id == LEFT_ARM) {
goal.data_array = {goal.data_array[0], goal.data_array[1], goal.data_array[2],
goal.data_array[3], goal.data_array[4], goal.data_array[5], goal.data_array[6]};
} else if (goal.body_id == RIGHT_ARM) {
goal.data_array = {goal.data_array[7], goal.data_array[8], goal.data_array[9],
goal.data_array[10], goal.data_array[11], goal.data_array[12], goal.data_array[13]};
}
}
goal.data_length = POSE_DIMENSION;
goal.frame_time_stamp = this->get_clock()->now().nanoseconds();
if (goal.data_type == ARM_COMMAND_TYPE_POSE_DIRECT_MOVE) {
if (goal.data_length == 14 && goal.data_array.size() == 14) {
if (goal.body_id == LEFT_ARM) {
goal.data_array = {goal.data_array[0], goal.data_array[1], goal.data_array[2],
goal.data_array[3], goal.data_array[4], goal.data_array[5], goal.data_array[6]};
} else if (goal.body_id == RIGHT_ARM) {
goal.data_array = {goal.data_array[7], goal.data_array[8], goal.data_array[9],
goal.data_array[10], goal.data_array[11], goal.data_array[12], goal.data_array[13]};
}
}
goal.data_length = POSE_DIMENSION;
} else {
if (goal.data_length == 12 && goal.data_array.size() == 12) {
if (goal.body_id == LEFT_ARM) {
goal.data_array = {goal.data_array[0], goal.data_array[1], goal.data_array[2],
goal.data_array[3], goal.data_array[4], goal.data_array[5]};
} else if (goal.body_id == RIGHT_ARM) {
goal.data_array = {goal.data_array[6], goal.data_array[7], goal.data_array[8], goal.data_array[9],
goal.data_array[10], goal.data_array[11]};
}
}
goal.data_length = USED_ARM_DOF;
goal.data_type = ARM_COMMAND_TYPE_ANGLE_DIRECT_MOVE;
}
RCLCPP_INFO(this->get_logger(), "[%s] Control params: body_id=%d, data_type=%d, data_length=%d, command_id=%ld, frame_time_stamp=%ld, data_array=[%.6f, %.6f, %.6f, %.6f, %.6f, %.6f, %.6f]",
skill_name.c_str(), goal.body_id, goal.data_type, goal.data_length, goal.command_id, goal.frame_time_stamp,
goal.data_array[0], goal.data_array[1], goal.data_array[2], goal.data_array[3], goal.data_array[4], goal.data_array[5], goal.data_array[6]);