From e8ecc73b698dd924bd060b14b80579e908bd228a Mon Sep 17 00:00:00 2001 From: david Date: Tue, 28 Oct 2025 10:59:06 +0800 Subject: [PATCH] support arm movej and movep --- .../config/bt_carry_boxes_sch1.params.yaml | 30 +++++++-------- .../config/bt_carry_boxes_sch2.params.yaml | 24 ++++++------ src/brain/src/cerebellum_node.cpp | 38 ++++++++++++------- 3 files changed, 52 insertions(+), 40 deletions(-) diff --git a/src/brain/config/bt_carry_boxes_sch1.params.yaml b/src/brain/config/bt_carry_boxes_sch1.params.yaml index 57b2f97..bbd7080 100644 --- a/src/brain/config/bt_carry_boxes_sch1.params.yaml +++ b/src/brain/config/bt_carry_boxes_sch1.params.yaml @@ -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] ' diff --git a/src/brain/config/bt_carry_boxes_sch2.params.yaml b/src/brain/config/bt_carry_boxes_sch2.params.yaml index a8f57f1..4f8b5df 100644 --- a/src/brain/config/bt_carry_boxes_sch2.params.yaml +++ b/src/brain/config/bt_carry_boxes_sch2.params.yaml @@ -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] ' diff --git a/src/brain/src/cerebellum_node.cpp b/src/brain/src/cerebellum_node.cpp index e26453e..345f27d 100644 --- a/src/brain/src/cerebellum_node.cpp +++ b/src/brain/src/cerebellum_node.cpp @@ -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]);