From fc5279ea1660b1680e520f77a5bba1b9a18f1b8a Mon Sep 17 00:00:00 2001 From: 2194555 <3631676002@qq.com> Date: Fri, 22 Mar 2024 22:43:46 +0800 Subject: [PATCH 01/25] Add small island get stone. --- engineer_middleware/config/steps_list.yaml | 133 ++++++++++++++++++++- 1 file changed, 132 insertions(+), 1 deletion(-) diff --git a/engineer_middleware/config/steps_list.yaml b/engineer_middleware/config/steps_list.yaml index 60b1cdd..2113ebc 100644 --- a/engineer_middleware/config/steps_list.yaml +++ b/engineer_middleware/config/steps_list.yaml @@ -27,8 +27,12 @@ common: mechanical: down_position: &JOINT1_DOWN_POSITION 0.015 + ready_position: &JOINT1_READY_POSITION + 0.145 up_position: &JOINT1_UP_POSITION - 0.22 + 0.252 + ready_store: &JOINT1_READY_STORE_POSITION + 0.162 home: zero_stone_position: &JOINT1_ZERO_STONE_POSITION 0.08 @@ -49,6 +53,10 @@ common: 0.2575 exchange_position: &JOINT2_EXCHANGE_POSITION 0.07 + lift_stone_position: &JOINT2_LIFT_STONE_POSITION + 0.085 + store_stone_position: &JOINT2_STORE_STONE_POSITION + 0.02 joint3: mechanical: @@ -58,6 +66,11 @@ common: 0.0001 right_position: &JOINT3_R_POSITION -0.275 + ready_position: &JOINT3_READY_POSITION + 0.178 + get_stone_position: &JOINT3_GET_STONE_POSITION + 0.034 + joint4: mechanical: @@ -380,3 +393,121 @@ steps_list: # common: # <<: *NORMALLY + ###################SMALL_ISLAND_FROM_TOP################ + ONE_STONE_SMALL_ISLAND: + - gimbal: + <<: + - step: "SI_ARM_READY" + arm: + joints: [ *JOINT1_READY_POSITION, *JOINT2_BACK_POSITION, *JOINT3_READY_POSITION, *JOINT4_R_POSITION, *JOINT5_MID_POSITION, *JOINT6_MID_POSITION, *JOINT7_MID_POSITION ] + common: + <<: *NORMALLY + tolerance: + <<: *NORMAL_TOLERANCE + - step: "OPEN_GRIPPER" + gripper: + <<: *OPEN_GRIPPER + - step: "SI_GET_STONE" + arm: + joints: [ *JOINT1_READY_POSITION, *JOINT2_BACK_POSITION, *JOINT3_GET_STONE_POSITION, *JOINT4_R_POSITION, *JOINT5_MID_POSITION, *JOINT6_MID_POSITION, *JOINT7_MID_POSITION ] + common: + <<: *NORMALLY + tolerance: + <<: *NORMAL_TOLERANCE + - step: "JOINT1_UP" + arm: + joints: [ *JOINT1_UP_POSITION, "KEEP", "KEEP", "KEEP", "KEEP", "KEEP", "KEEP"] + common: + <<: *NORMALLY + tolerance: + <<: *NORMAL_TOLERANCE + - step: "SI_ARM_LIFT_STONE" + arm: + joints: [ *JOINT1_UP_POSITION, *JOINT2_LIFT_STONE_POSITION, *JOINT3_GET_STONE_POSITION, *JOINT4_R_POSITION, *JOINT5_MID_POSITION, *JOINT6_UP_POSITION, *JOINT7_MID_POSITION ] + common: + <<: *NORMALLY + tolerance: + <<: *NORMAL_TOLERANCE + - step: "READY_TO_STORE_STONE" + arm: + joints: [ *JOINT1_READY_STORE_POSITION, *JOINT2_LIFT_STONE_POSITION, *JOINT3_L_POSITION, *JOINT4_R_POSITION, *JOINT5_MID_POSITION, *JOINT6_UP_POSITION, *JOINT7_MID_POSITION ] + common: + <<: *NORMALLY + tolerance: + <<: *NORMAL_TOLERANCE + - step: "ORE_LIFTER_UP" + ore_lifter: + + - step: "STORE_STONE" + arm: + joints: [ *JOINT1_READY_STORE_POSITION, *JOINT2_STORE_STONE_POSITION, *JOINT3_L_POSITION, *JOINT4_R_POSITION, *JOINT5_R90_POSITION, *JOINT6_UP_POSITION, *JOINT7_MID_POSITION ] + common: + <<: *NORMALLY + tolerance: + <<: *NORMAL_TOLERANCE + - step: "CLOSE_GRIPPER" + gripper: + <<: *CLOSE_GRIPPER + - step: "ARM_RESET" + arm: + joints: [ *JOINT1_READY_STORE_POSITION, *JOINT2_STORE_STONE_POSITION, *JOINT3_L_POSITION, *JOINT4_R_POSITION, *JOINT5_MID_POSITION, *JOINT6_MID_POSITION, *JOINT7_MID_POSITION ] + common: + <<: *NORMALLY + tolerance: + <<: *NORMAL_TOLERANCE + + #################BIG_ISLAND############# +# ONE_STONE_BIG_ISLAND： +# - gimbal: +# <<: *NORMALLY +# - step: "BI_ARM_READY" +# arm: +# joints: [ ] +# common: +# <<: *NORMALLY +# tolerance: +# <<: *NORMAL_TOLERANCE +# - step: "OPEN_GRIPPER" +# gripper: +# <<: *OPEN_GRIPPER +# - step: "BI_GET_STONE" +# arm: +# joints: [ ] +# common: +# <<: *NORMALLY +# tolerance: +# <<: *NORMAL_TOLERANCE +# - step: "PULL_OUT_STONE" +# arm: +# joints: [ ] +# common: +# <<: *NORMALLY +# tolerance: +# <<: *NORMAL_TOLERANCE +# - step: "BI_ARM_UP_STONE" #########joint6抬起，无需转动joint7####### +# arm: +# joints: [ ] +# common: +# <<: *NORMALLY +# tolerance: +# <<: *NORMAL_TOLERANCE +# - step: "ORE_LIFTER_UP" +# ore_lifter: +# +# - step: "STORE_STONE" +# arm: +# joints: [ ] +# common: +# <<: *NORMALLY +# tolerance: +# <<: *NORMAL_TOLERANCE +# - step: "CLOSE_GRIPPER" +# gripper: +# <<: *CLOSE_GRIPPER +# - step: "ARM_RESET" +# arm: +# joints: [ ] +# common: +# <<: *NORMALLY +# tolerance: +# <<: *NORMAL_TOLERANCE \ No newline at end of file From 1476e70504a506657d85ee027266acca382780f6 Mon Sep 17 00:00:00 2001 From: 2194555 <3631676002@qq.com> Date: Fri, 22 Mar 2024 23:06:27 +0800 Subject: [PATCH 02/25] Update. --- engineer_middleware/config/steps_list.yaml | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/engineer_middleware/config/steps_list.yaml b/engineer_middleware/config/steps_list.yaml index 2113ebc..82e2d29 100644 --- a/engineer_middleware/config/steps_list.yaml +++ b/engineer_middleware/config/steps_list.yaml @@ -395,7 +395,10 @@ steps_list: ###################SMALL_ISLAND_FROM_TOP################ ONE_STONE_SMALL_ISLAND: - - gimbal: + - step: "GIMBAL_READY" + gimbal: + <<: *SIDE_POS + gimbal_lifter: <<: - step: "SI_ARM_READY" arm: From 5f3cd6d40adae87117920dfe856be6f108ae4330 Mon Sep 17 00:00:00 2001 From: 2194555 <3631676002@qq.com> Date: Sat, 23 Mar 2024 17:27:04 +0800 Subject: [PATCH 03/25] Add small island two stone. --- engineer_middleware/config/steps_list.yaml | 222 ++++++++++++++++----- 1 file changed, 169 insertions(+), 53 deletions(-) diff --git a/engineer_middleware/config/steps_list.yaml b/engineer_middleware/config/steps_list.yaml index 82e2d29..8c295fd 100644 --- a/engineer_middleware/config/steps_list.yaml +++ b/engineer_middleware/config/steps_list.yaml @@ -28,9 +28,9 @@ common: down_position: &JOINT1_DOWN_POSITION 0.015 ready_position: &JOINT1_READY_POSITION - 0.145 + 0.175 up_position: &JOINT1_UP_POSITION - 0.252 + 0.312 ready_store: &JOINT1_READY_STORE_POSITION 0.162 home: @@ -67,7 +67,7 @@ common: right_position: &JOINT3_R_POSITION -0.275 ready_position: &JOINT3_READY_POSITION - 0.178 + 0.098 get_stone_position: &JOINT3_GET_STONE_POSITION 0.034 @@ -140,52 +140,53 @@ common: <<: *NORMAL_TOLERANCE - reversal: - velocity_stop: &VELOCITY_STOP - mode: "VELOCITY" - values: [ 0.,0.,0.,0.,0.,0. ] - velocity_z_out: &VELOCITY_Z_OUT - mode: "VELOCITY" - values: [ 0.,0.,3.,0.,0.,0. ] - velocity_z_out_store: &VELOCITY_Z_OUT_STORE - mode: "VELOCITY" - values: [ 0.,0.,0.2,0.,0.,0. ] - velocity_z_in: &VELOCITY_Z_IN - mode: "VELOCITY" - values: [ 0.,0.,-1.2,0.,-0.01,0. ] - velocity_z_in_quickly: &VELOCITY_Z_IN_QUICKLY - mode: "VELOCITY" - values: [ 0.,0.,-3.0,0.,-0.01,0. ] - velocity_z_in_quickly_with_delay: &VELOCITY_Z_IN_QUICKLY_WITH_DELAY - mode: "VELOCITY" - values: [ 0.,0.,-3.0,0.,-0.01,0. ] - delay: 0.3 - position_z_out: &POSITION_Z_OUT - mode: "POSITION" - values: [ 0.,0.,7.,0.,0.,0. ] - delay: 0.2 - position_z_out_little: &POSITION_Z_OUT_LITTLE - mode: "POSITION" - values: [ 0.,0.,3.,0.,0.,0. ] - delay: 0.4 + reversal: + velocity_stop: &VELOCITY_STOP + mode: "VELOCITY" + values: [ 0.,0.,0.,0.,0.,0. ] + velocity_z_out: &VELOCITY_Z_OUT + mode: "VELOCITY" + values: [ 0.,0.,3.,0.,0.,0. ] + velocity_z_out_store: &VELOCITY_Z_OUT_STORE + mode: "VELOCITY" + values: [ 0.,0.,0.2,0.,0.,0. ] + velocity_z_in: &VELOCITY_Z_IN + mode: "VELOCITY" + values: [ 0.,0.,-1.2,0.,-0.01,0. ] + velocity_z_in_quickly: &VELOCITY_Z_IN_QUICKLY + mode: "VELOCITY" + values: [ 0.,0.,-3.0,0.,-0.01,0. ] + velocity_z_in_quickly_with_delay: &VELOCITY_Z_IN_QUICKLY_WITH_DELAY + mode: "VELOCITY" + values: [ 0.,0.,-3.0,0.,-0.01,0. ] + delay: 0.3 + position_z_out: &POSITION_Z_OUT + mode: "POSITION" + values: [ 0.,0.,7.,0.,0.,0. ] + delay: 0.2 + position_z_out_little: &POSITION_Z_OUT_LITTLE + mode: "POSITION" + values: [ 0.,0.,3.,0.,0.,0. ] + delay: 0.4 - chassis_move: - chassis_backward_30: &CHASSIS_BACKWARD_25 - frame: base_link - position: [ -0.35,0. ] - yaw: 0.0 - chassis_tolerance_position: 0.1 - chassis_tolerance_angular: 0.3 - common: - timeout: 2. - chassis_180: &CHASSIS_180 - frame: base_link - position: [ 0., 0. ] - yaw: 2.80 - chassis_tolerance_position: 0.1 - chassis_tolerance_angular: 0.2 - common: - timeout: 2. + chassis_move: + chassis_backward_30: &CHASSIS_BACKWARD_25 + frame: base_link + position: [ -0.35,0. ] + yaw: 0.0 + chassis_tolerance_position: 0.1 + chassis_tolerance_angular: 0.3 + common: + timeout: 2. + chassis_180: &CHASSIS_180 + frame: base_link + position: [ 0., 0. ] + yaw: 2.80 + chassis_tolerance_position: 0.1 + chassis_tolerance_angular: 0.2 + common: + timeout: + 2. steps_list: ###################### SMALL_ISLAND ####################### @@ -398,8 +399,6 @@ steps_list: - step: "GIMBAL_READY" gimbal: <<: *SIDE_POS - gimbal_lifter: - <<: - step: "SI_ARM_READY" arm: joints: [ *JOINT1_READY_POSITION, *JOINT2_BACK_POSITION, *JOINT3_READY_POSITION, *JOINT4_R_POSITION, *JOINT5_MID_POSITION, *JOINT6_MID_POSITION, *JOINT7_MID_POSITION ] @@ -410,6 +409,7 @@ steps_list: - step: "OPEN_GRIPPER" gripper: <<: *OPEN_GRIPPER + ONE_STONE_SMALL_ISLAND0: - step: "SI_GET_STONE" arm: joints: [ *JOINT1_READY_POSITION, *JOINT2_BACK_POSITION, *JOINT3_GET_STONE_POSITION, *JOINT4_R_POSITION, *JOINT5_MID_POSITION, *JOINT6_MID_POSITION, *JOINT7_MID_POSITION ] @@ -438,9 +438,6 @@ steps_list: <<: *NORMALLY tolerance: <<: *NORMAL_TOLERANCE - - step: "ORE_LIFTER_UP" - ore_lifter: - - step: "STORE_STONE" arm: joints: [ *JOINT1_READY_STORE_POSITION, *JOINT2_STORE_STONE_POSITION, *JOINT3_L_POSITION, *JOINT4_R_POSITION, *JOINT5_R90_POSITION, *JOINT6_UP_POSITION, *JOINT7_MID_POSITION ] @@ -459,6 +456,125 @@ steps_list: tolerance: <<: *NORMAL_TOLERANCE + ####FIRST_STONE### + TWO_STONE_SMALL_ISLAND: + - step: "GIMBAL_READY" + gimbal: + <<: *SIDE_POS + - step: "SI_ARM_READY" + arm: + joints: [ *JOINT1_READY_POSITION, *JOINT2_BACK_POSITION, *JOINT3_READY_POSITION, *JOINT4_R_POSITION, *JOINT5_MID_POSITION, *JOINT6_MID_POSITION, *JOINT7_MID_POSITION ] + common: + <<: *NORMALLY + tolerance: + <<: *NORMAL_TOLERANCE + - step: "OPEN_GRIPPER" + gripper: + <<: *OPEN_GRIPPER + TWO_STONE_SMALL_ISLAND0: + - step: "SI_GET_STONE" + arm: + joints: [ *JOINT1_READY_POSITION, *JOINT2_BACK_POSITION, *JOINT3_GET_STONE_POSITION, *JOINT4_R_POSITION, *JOINT5_MID_POSITION, *JOINT6_MID_POSITION, *JOINT7_MID_POSITION ] + common: + <<: *NORMALLY + tolerance: + <<: *NORMAL_TOLERANCE + - step: "JOINT1_UP" + arm: + joints: [ *JOINT1_UP_POSITION, "KEEP", "KEEP", "KEEP", "KEEP", "KEEP", "KEEP"] + common: + <<: *NORMALLY + tolerance: + <<: *NORMAL_TOLERANCE + - step: "SI_ARM_LIFT_STONE" + arm: + joints: [ *JOINT1_UP_POSITION, *JOINT2_LIFT_STONE_POSITION, *JOINT3_GET_STONE_POSITION, *JOINT4_R_POSITION, *JOINT5_MID_POSITION, *JOINT6_UP_POSITION, *JOINT7_MID_POSITION ] + common: + <<: *NORMALLY + tolerance: + <<: *NORMAL_TOLERANCE + - step: "READY_TO_STORE_STONE" + arm: + joints: [ *JOINT1_READY_STORE_POSITION, *JOINT2_LIFT_STONE_POSITION, *JOINT3_L_POSITION, *JOINT4_R_POSITION, *JOINT5_MID_POSITION, *JOINT6_UP_POSITION, *JOINT7_MID_POSITION ] + common: + <<: *NORMALLY + tolerance: + <<: *NORMAL_TOLERANCE + - step: "STORE_STONE" + arm: + joints: [ *JOINT1_READY_STORE_POSITION, *JOINT2_STORE_STONE_POSITION, *JOINT3_L_POSITION, *JOINT4_R_POSITION, *JOINT5_R90_POSITION, *JOINT6_UP_POSITION, *JOINT7_MID_POSITION ] + common: + <<: *NORMALLY + tolerance: + <<: *NORMAL_TOLERANCE + - step: "CLOSE_GRIPPER" + gripper: + <<: *CLOSE_GRIPPER + - step: "ARM_RESET" + arm: + joints: [ *JOINT1_READY_STORE_POSITION, *JOINT2_STORE_STONE_POSITION, *JOINT3_L_POSITION, *JOINT4_R_POSITION, *JOINT5_MID_POSITION, *JOINT6_MID_POSITION, *JOINT7_MID_POSITION ] + common: + <<: *NORMALLY + tolerance: + <<: *NORMAL_TOLERANCE + ###SECOND_STONE### + TWO_STONE_SMALL_ISLAND00: + - step: "SI_ARM_READY" + arm: + joints: [ *JOINT1_READY_POSITION, *JOINT2_FURTHEST_POSITION, *JOINT3_READY_POSITION, *JOINT4_R_POSITION, *JOINT5_MID_POSITION, *JOINT6_MID_POSITION, *JOINT7_MID_POSITION ] + common: + <<: *NORMALLY + tolerance: + <<: *NORMAL_TOLERANCE + - step: "OPEN_GRIPPER" + gripper: + <<: *OPEN_GRIPPER + TWO_STONE_SMALL_ISLAND000: + - step: "SI_GET_STONE" + arm: + joints: [ *JOINT1_READY_POSITION, *JOINT2_FURTHEST_POSITION, *JOINT3_GET_STONE_POSITION, *JOINT4_R_POSITION, *JOINT5_MID_POSITION, *JOINT6_MID_POSITION, *JOINT7_MID_POSITION ] + common: + <<: *NORMALLY + tolerance: + <<: *NORMAL_TOLERANCE + - step: "JOINT1_UP" + arm: + joints: [ *JOINT1_UP_POSITION, "KEEP", "KEEP", "KEEP", "KEEP", "KEEP", "KEEP"] + common: + <<: *NORMALLY + tolerance: + <<: *NORMAL_TOLERANCE + - step: "SI_ARM_LIFT_STONE" + arm: + joints: [ *JOINT1_UP_POSITION, *JOINT2_LIFT_STONE_POSITION, *JOINT3_GET_STONE_POSITION, *JOINT4_R_POSITION, *JOINT5_MID_POSITION, *JOINT6_UP_POSITION, *JOINT7_MID_POSITION ] + common: + <<: *NORMALLY + tolerance: + <<: *NORMAL_TOLERANCE + - step: "READY_TO_STORE_STONE" + arm: + joints: [ *JOINT1_READY_STORE_POSITION, *JOINT2_LIFT_STONE_POSITION, *JOINT3_L_POSITION, *JOINT4_R_POSITION, *JOINT5_MID_POSITION, *JOINT6_UP_POSITION, *JOINT7_MID_POSITION ] + common: + <<: *NORMALLY + tolerance: + <<: *NORMAL_TOLERANCE + - step: "STORE_STONE" + arm: + joints: [ *JOINT1_READY_STORE_POSITION, *JOINT2_STORE_STONE_POSITION, *JOINT3_L_POSITION, *JOINT4_R_POSITION, *JOINT5_R90_POSITION, *JOINT6_UP_POSITION, *JOINT7_MID_POSITION ] + common: + <<: *NORMALLY + tolerance: + <<: *NORMAL_TOLERANCE + - step: "CLOSE_GRIPPER" + gripper: + <<: *CLOSE_GRIPPER + - step: "ARM_RESET" + arm: + joints: [ *JOINT1_READY_STORE_POSITION, *JOINT2_STORE_STONE_POSITION, *JOINT3_L_POSITION, *JOINT4_R_POSITION, *JOINT5_MID_POSITION, *JOINT6_MID_POSITION, *JOINT7_MID_POSITION ] + common: + <<: *NORMALLY + tolerance: + <<: *NORMAL_TOLERANCE #################BIG_ISLAND############# # ONE_STONE_BIG_ISLAND： # - gimbal: From a834134b37e9d542e91cdcb690d1230ddee187e8 Mon Sep 17 00:00:00 2001 From: 2194555 <3631676002@qq.com> Date: Sun, 24 Mar 2024 16:01:33 +0800 Subject: [PATCH 04/25] Update small island two stone. --- engineer_middleware/config/steps_list.yaml | 44 +++++++++++++++++----- 1 file changed, 35 insertions(+), 9 deletions(-) diff --git a/engineer_middleware/config/steps_list.yaml b/engineer_middleware/config/steps_list.yaml index 8c295fd..1e4490a 100644 --- a/engineer_middleware/config/steps_list.yaml +++ b/engineer_middleware/config/steps_list.yaml @@ -50,13 +50,15 @@ common: get_ore_position: &JOINT2_GET_ORE_POSITION 0.1 furthest_position: &JOINT2_FURTHEST_POSITION - 0.2575 + 0.232 exchange_position: &JOINT2_EXCHANGE_POSITION 0.07 lift_stone_position: &JOINT2_LIFT_STONE_POSITION 0.085 store_stone_position: &JOINT2_STORE_STONE_POSITION 0.02 + ready_second_position: &JOINT2_READY_SECOND_POSITION + 0.142 joint3: mechanical: @@ -68,8 +70,12 @@ common: -0.275 ready_position: &JOINT3_READY_POSITION 0.098 + ready_second_position: &JOINT3_READY_SECOND_POSITION + 0.03 get_stone_position: &JOINT3_GET_STONE_POSITION 0.034 + get_second_position: &JOINT3_GET_SECOND_POSITION + -0.018 joint4: @@ -80,6 +86,8 @@ common: 0.001 right_position: &JOINT4_R_POSITION -1.45 + ready_second_position: &JOINT4_READY_SECOND_POSITION + -0.777 joint5: mechanical: mid_position: &JOINT5_MID_POSITION @@ -96,6 +104,8 @@ common: -1.49 down_position: &JOINT6_DOWN_POSITION 1.569 + ready_second_position: &JOINT6_READY_SECOND_POSITION + 0.694 joint7: mechanical: mid_position: &JOINT7_MID_POSITION @@ -185,8 +195,7 @@ common: chassis_tolerance_position: 0.1 chassis_tolerance_angular: 0.2 common: - timeout: - 2. + timeout: 2. steps_list: ###################### SMALL_ISLAND ####################### @@ -312,7 +321,7 @@ steps_list: spacial_shape: SPHERE radius: 0.5 point_resolution: 0.5 - max_planning_times: 100 + max_planning_times: 1 common: <<: *QUICKLY @@ -329,9 +338,26 @@ steps_list: spacial_shape: SPHERE radius: 0.5 point_resolution: 0.5 - max_planning_times: 100 + max_planning_times: 1 common: <<: *NORMALLY + + TEST_ORI: + - step: "ori test" + arm: + frame: exchanger + rpy: [ 0., -1.5707963, 0. ] + tolerance_orientation: 0.001 + + TEST_ORI2: + - step: "ori test2" + arm: + frame: exchanger + rpy: [ 0., -1.5707963, 0. ] + xyz: [ -0.3, 0.2, -0.2 ] + tolerance_position: 0.01 + tolerance_orientation: 0.001 + EXCHANGE_ORI: - step: "test new exchange" arm: @@ -344,7 +370,7 @@ steps_list: spacial_shape: SPHERE radius: 0.5 point_resolution: 0.5 - max_planning_times: 100 + max_planning_times: 1 common: <<: *NORMALLY EXCHANGE_STRAIGHT: @@ -360,7 +386,7 @@ steps_list: spacial_shape: SPHERE radius: 0.5 point_resolution: 0.5 - max_planning_times: 100 + max_planning_times: 1 common: <<: *NORMALLY @@ -521,7 +547,7 @@ steps_list: TWO_STONE_SMALL_ISLAND00: - step: "SI_ARM_READY" arm: - joints: [ *JOINT1_READY_POSITION, *JOINT2_FURTHEST_POSITION, *JOINT3_READY_POSITION, *JOINT4_R_POSITION, *JOINT5_MID_POSITION, *JOINT6_MID_POSITION, *JOINT7_MID_POSITION ] + joints: [ *JOINT1_READY_POSITION, *JOINT2_READY_SECOND_POSITION, *JOINT3_READY_SECOND_POSITION, *JOINT4_READY_SECOND_POSITION, *JOINT5_L90_POSITION, *JOINT6_READY_SECOND_POSITION, *JOINT7_R90_POSITION ] common: <<: *NORMALLY tolerance: @@ -532,7 +558,7 @@ steps_list: TWO_STONE_SMALL_ISLAND000: - step: "SI_GET_STONE" arm: - joints: [ *JOINT1_READY_POSITION, *JOINT2_FURTHEST_POSITION, *JOINT3_GET_STONE_POSITION, *JOINT4_R_POSITION, *JOINT5_MID_POSITION, *JOINT6_MID_POSITION, *JOINT7_MID_POSITION ] + joints: [ *JOINT1_READY_POSITION, *JOINT2_READY_SECOND_POSITION, *JOINT3_GET_SECOND_POSITION, *JOINT4_READY_SECOND_POSITION, *JOINT5_L90_POSITION, *JOINT6_READY_SECOND_POSITION, *JOINT7_R90_POSITION ] common: <<: *NORMALLY tolerance: From fd2ee5e59bb2155d7ee18ca0ce3d10596fee5a35 Mon Sep 17 00:00:00 2001 From: 2194555 <3631676002@qq.com> Date: Mon, 25 Mar 2024 00:51:01 +0800 Subject: [PATCH 05/25] Update. --- engineer_middleware/config/steps_list.yaml | 26 +++++++++++++++------- 1 file changed, 18 insertions(+), 8 deletions(-) diff --git a/engineer_middleware/config/steps_list.yaml b/engineer_middleware/config/steps_list.yaml index 1e4490a..91d8342 100644 --- a/engineer_middleware/config/steps_list.yaml +++ b/engineer_middleware/config/steps_list.yaml @@ -28,7 +28,7 @@ common: down_position: &JOINT1_DOWN_POSITION 0.015 ready_position: &JOINT1_READY_POSITION - 0.175 + 0.165 up_position: &JOINT1_UP_POSITION 0.312 ready_store: &JOINT1_READY_STORE_POSITION @@ -76,6 +76,8 @@ common: 0.034 get_second_position: &JOINT3_GET_SECOND_POSITION -0.018 + store_stone_position: &JOINT3_STORE_STONE_POSITION + 0.30 joint4: @@ -101,7 +103,7 @@ common: mid_position: &JOINT6_MID_POSITION 0.001 up_position: &JOINT6_UP_POSITION - -1.49 + -1.569 down_position: &JOINT6_DOWN_POSITION 1.569 ready_second_position: &JOINT6_READY_SECOND_POSITION @@ -459,14 +461,22 @@ steps_list: <<: *NORMAL_TOLERANCE - step: "READY_TO_STORE_STONE" arm: - joints: [ *JOINT1_READY_STORE_POSITION, *JOINT2_LIFT_STONE_POSITION, *JOINT3_L_POSITION, *JOINT4_R_POSITION, *JOINT5_MID_POSITION, *JOINT6_UP_POSITION, *JOINT7_MID_POSITION ] + joints: [ *JOINT1_UP_POSITION, *JOINT2_LIFT_STONE_POSITION, *JOINT3_STORE_STONE_POSITION, *JOINT4_R_POSITION, *JOINT5_MID_POSITION, *JOINT6_UP_POSITION, *JOINT7_MID_POSITION ] common: <<: *NORMALLY tolerance: <<: *NORMAL_TOLERANCE + ONE_STONE_SMALL_ISLAND00: - step: "STORE_STONE" arm: - joints: [ *JOINT1_READY_STORE_POSITION, *JOINT2_STORE_STONE_POSITION, *JOINT3_L_POSITION, *JOINT4_R_POSITION, *JOINT5_R90_POSITION, *JOINT6_UP_POSITION, *JOINT7_MID_POSITION ] + joints: [ *JOINT1_UP_POSITION, *JOINT2_STORE_STONE_POSITION, *JOINT3_STORE_STONE_POSITION, *JOINT4_R_POSITION, *JOINT5_R90_POSITION, *JOINT6_UP_POSITION, *JOINT7_MID_POSITION ] + common: + <<: *NORMALLY + tolerance: + <<: *NORMAL_TOLERANCE + - step: "JOINT1_DOWN" + arm: + joints: [ *JOINT1_READY_STORE_POSITION, "KEEP", "KEEP", "KEEP", "KEEP", "KEEP", "KEEP" ] common: <<: *NORMALLY tolerance: @@ -521,14 +531,14 @@ steps_list: <<: *NORMAL_TOLERANCE - step: "READY_TO_STORE_STONE" arm: - joints: [ *JOINT1_READY_STORE_POSITION, *JOINT2_LIFT_STONE_POSITION, *JOINT3_L_POSITION, *JOINT4_R_POSITION, *JOINT5_MID_POSITION, *JOINT6_UP_POSITION, *JOINT7_MID_POSITION ] + joints: [ *JOINT1_READY_STORE_POSITION, *JOINT2_LIFT_STONE_POSITION, *JOINT3_STORE_STONE_POSITION, *JOINT4_R_POSITION, *JOINT5_MID_POSITION, *JOINT6_UP_POSITION, *JOINT7_MID_POSITION ] common: <<: *NORMALLY tolerance: <<: *NORMAL_TOLERANCE - step: "STORE_STONE" arm: - joints: [ *JOINT1_READY_STORE_POSITION, *JOINT2_STORE_STONE_POSITION, *JOINT3_L_POSITION, *JOINT4_R_POSITION, *JOINT5_R90_POSITION, *JOINT6_UP_POSITION, *JOINT7_MID_POSITION ] + joints: [ *JOINT1_READY_STORE_POSITION, *JOINT2_STORE_STONE_POSITION, *JOINT3_STORE_STONE_POSITION, *JOINT4_R_POSITION, *JOINT5_R90_POSITION, *JOINT6_UP_POSITION, *JOINT7_MID_POSITION ] common: <<: *NORMALLY tolerance: @@ -579,14 +589,14 @@ steps_list: <<: *NORMAL_TOLERANCE - step: "READY_TO_STORE_STONE" arm: - joints: [ *JOINT1_READY_STORE_POSITION, *JOINT2_LIFT_STONE_POSITION, *JOINT3_L_POSITION, *JOINT4_R_POSITION, *JOINT5_MID_POSITION, *JOINT6_UP_POSITION, *JOINT7_MID_POSITION ] + joints: [ *JOINT1_READY_STORE_POSITION, *JOINT2_LIFT_STONE_POSITION, *JOINT3_STORE_STONE_POSITION, *JOINT4_R_POSITION, *JOINT5_MID_POSITION, *JOINT6_UP_POSITION, *JOINT7_MID_POSITION ] common: <<: *NORMALLY tolerance: <<: *NORMAL_TOLERANCE - step: "STORE_STONE" arm: - joints: [ *JOINT1_READY_STORE_POSITION, *JOINT2_STORE_STONE_POSITION, *JOINT3_L_POSITION, *JOINT4_R_POSITION, *JOINT5_R90_POSITION, *JOINT6_UP_POSITION, *JOINT7_MID_POSITION ] + joints: [ *JOINT1_READY_STORE_POSITION, *JOINT2_STORE_STONE_POSITION, *JOINT3_STORE_STONE_POSITION, *JOINT4_R_POSITION, *JOINT5_R90_POSITION, *JOINT6_UP_POSITION, *JOINT7_MID_POSITION ] common: <<: *NORMALLY tolerance: From 198da3429022a69463ccdf03116cc4feb22bad6f Mon Sep 17 00:00:00 2001 From: 2194555 <3631676002@qq.com> Date: Tue, 26 Mar 2024 21:09:02 +0800 Subject: [PATCH 06/25] Update with tested. --- engineer_middleware/config/steps_list.yaml | 44 +++++++++------------- 1 file changed, 18 insertions(+), 26 deletions(-) diff --git a/engineer_middleware/config/steps_list.yaml b/engineer_middleware/config/steps_list.yaml index 91d8342..26d4695 100644 --- a/engineer_middleware/config/steps_list.yaml +++ b/engineer_middleware/config/steps_list.yaml @@ -15,7 +15,7 @@ common: tolerance: small_tolerance: &SMALL_TOLERANCE - tolerance_joints: [ 0.005, 0.008, 0.015, 0.1, 0.1, 0.1 ] + tolerance_joints: [ 0.005, 0.008, 0.015, 0.1, 0.1, 0.1, 0.05 ] normal_tolerance: &NORMAL_TOLERANCE tolerance_joints: [ 0.01, 0.010, 0.015, 0.3, 0.2, 0.15, 0.1 ] bigger_tolerance: &BIGGER_TOLERANCE @@ -28,7 +28,7 @@ common: down_position: &JOINT1_DOWN_POSITION 0.015 ready_position: &JOINT1_READY_POSITION - 0.165 + 0.15 up_position: &JOINT1_UP_POSITION 0.312 ready_store: &JOINT1_READY_STORE_POSITION @@ -87,7 +87,7 @@ common: mid_position: &JOINT4_MID_POSITION 0.001 right_position: &JOINT4_R_POSITION - -1.45 + -1.49 ready_second_position: &JOINT4_READY_SECOND_POSITION -0.777 joint5: @@ -115,7 +115,7 @@ common: left_90_position: &JOINT7_L90_POSITION -1.59 right_90_position: &JOINT7_R90_POSITION - 1.49 + 1.57 gimbal: side_pos: &SIDE_POS frame: gimbal_lifter @@ -456,31 +456,23 @@ steps_list: arm: joints: [ *JOINT1_UP_POSITION, *JOINT2_LIFT_STONE_POSITION, *JOINT3_GET_STONE_POSITION, *JOINT4_R_POSITION, *JOINT5_MID_POSITION, *JOINT6_UP_POSITION, *JOINT7_MID_POSITION ] common: - <<: *NORMALLY + <<: *SLOWLY tolerance: <<: *NORMAL_TOLERANCE - step: "READY_TO_STORE_STONE" arm: - joints: [ *JOINT1_UP_POSITION, *JOINT2_LIFT_STONE_POSITION, *JOINT3_STORE_STONE_POSITION, *JOINT4_R_POSITION, *JOINT5_MID_POSITION, *JOINT6_UP_POSITION, *JOINT7_MID_POSITION ] + joints: [ *JOINT1_READY_STORE_POSITION, *JOINT2_LIFT_STONE_POSITION, *JOINT3_STORE_STONE_POSITION, *JOINT4_R_POSITION, *JOINT5_MID_POSITION, *JOINT6_UP_POSITION, *JOINT7_MID_POSITION ] common: <<: *NORMALLY tolerance: - <<: *NORMAL_TOLERANCE - ONE_STONE_SMALL_ISLAND00: + <<: *SMALL_TOLERANCE - step: "STORE_STONE" arm: - joints: [ *JOINT1_UP_POSITION, *JOINT2_STORE_STONE_POSITION, *JOINT3_STORE_STONE_POSITION, *JOINT4_R_POSITION, *JOINT5_R90_POSITION, *JOINT6_UP_POSITION, *JOINT7_MID_POSITION ] - common: - <<: *NORMALLY - tolerance: - <<: *NORMAL_TOLERANCE - - step: "JOINT1_DOWN" - arm: - joints: [ *JOINT1_READY_STORE_POSITION, "KEEP", "KEEP", "KEEP", "KEEP", "KEEP", "KEEP" ] + joints: [ *JOINT1_READY_STORE_POSITION, *JOINT2_STORE_STONE_POSITION, *JOINT3_STORE_STONE_POSITION, *JOINT4_R_POSITION, *JOINT5_R90_POSITION, *JOINT6_UP_POSITION, *JOINT7_MID_POSITION ] common: - <<: *NORMALLY + <<: *SLOWLY tolerance: - <<: *NORMAL_TOLERANCE + <<: *SMALL_TOLERANCE - step: "CLOSE_GRIPPER" gripper: <<: *CLOSE_GRIPPER @@ -526,7 +518,7 @@ steps_list: arm: joints: [ *JOINT1_UP_POSITION, *JOINT2_LIFT_STONE_POSITION, *JOINT3_GET_STONE_POSITION, *JOINT4_R_POSITION, *JOINT5_MID_POSITION, *JOINT6_UP_POSITION, *JOINT7_MID_POSITION ] common: - <<: *NORMALLY + <<: *SLOWLY tolerance: <<: *NORMAL_TOLERANCE - step: "READY_TO_STORE_STONE" @@ -535,14 +527,14 @@ steps_list: common: <<: *NORMALLY tolerance: - <<: *NORMAL_TOLERANCE + <<: *SMALL_TOLERANCE - step: "STORE_STONE" arm: joints: [ *JOINT1_READY_STORE_POSITION, *JOINT2_STORE_STONE_POSITION, *JOINT3_STORE_STONE_POSITION, *JOINT4_R_POSITION, *JOINT5_R90_POSITION, *JOINT6_UP_POSITION, *JOINT7_MID_POSITION ] common: - <<: *NORMALLY + <<: *SLOWLY tolerance: - <<: *NORMAL_TOLERANCE + <<: *SMALL_TOLERANCE - step: "CLOSE_GRIPPER" gripper: <<: *CLOSE_GRIPPER @@ -561,7 +553,7 @@ steps_list: common: <<: *NORMALLY tolerance: - <<: *NORMAL_TOLERANCE + <<: *SMALL_TOLERANCE - step: "OPEN_GRIPPER" gripper: <<: *OPEN_GRIPPER @@ -572,7 +564,7 @@ steps_list: common: <<: *NORMALLY tolerance: - <<: *NORMAL_TOLERANCE + <<: *SMALL_TOLERANCE - step: "JOINT1_UP" arm: joints: [ *JOINT1_UP_POSITION, "KEEP", "KEEP", "KEEP", "KEEP", "KEEP", "KEEP"] @@ -593,14 +585,14 @@ steps_list: common: <<: *NORMALLY tolerance: - <<: *NORMAL_TOLERANCE + <<: *SMALL_TOLERANCE - step: "STORE_STONE" arm: joints: [ *JOINT1_READY_STORE_POSITION, *JOINT2_STORE_STONE_POSITION, *JOINT3_STORE_STONE_POSITION, *JOINT4_R_POSITION, *JOINT5_R90_POSITION, *JOINT6_UP_POSITION, *JOINT7_MID_POSITION ] common: <<: *NORMALLY tolerance: - <<: *NORMAL_TOLERANCE + <<: *SMALL_TOLERANCE - step: "CLOSE_GRIPPER" gripper: <<: *CLOSE_GRIPPER From b2c44501bac3212a7dfc0dbfb40cffef3d4f8913 Mon Sep 17 00:00:00 2001 From: cc0h <7921166012@qq.com> Date: Tue, 26 Mar 2024 22:09:04 +0800 Subject: [PATCH 07/25] Fix wrong member name. --- engineer_middleware/include/engineer_middleware/step.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/engineer_middleware/include/engineer_middleware/step.h b/engineer_middleware/include/engineer_middleware/step.h index 60fae69..e04ff2a 100644 --- a/engineer_middleware/include/engineer_middleware/step.h +++ b/engineer_middleware/include/engineer_middleware/step.h @@ -85,7 +85,7 @@ class Step if (step["scene_name"] == it->first) planning_scene_ = new PlanningScene(it->second, arm_group); } - if (step.hasMember("ore_rotater")) + if (step.hasMember("ore_rotator")) ore_rotate_motion_ = new JointPointMotion(step["ore_rotator"], ore_rotate_pub); if (step.hasMember("ore_lifter")) ore_lift_motion_ = new JointPointMotion(step["ore_lifter"], ore_lift_pub); From aeffc9fa794855fbee3471591599bf48934fd299 Mon Sep 17 00:00:00 2001 From: 2194555 <3631676002@qq.com> Date: Tue, 26 Mar 2024 22:54:27 +0800 Subject: [PATCH 08/25] Update ore_lifter and ore_rotator. --- engineer_middleware/config/steps_list.yaml | 51 ++++++++++++++++++++++ 1 file changed, 51 insertions(+) diff --git a/engineer_middleware/config/steps_list.yaml b/engineer_middleware/config/steps_list.yaml index 26d4695..850daae 100644 --- a/engineer_middleware/config/steps_list.yaml +++ b/engineer_middleware/config/steps_list.yaml @@ -199,6 +199,18 @@ common: common: timeout: 2. + ore_rotator: + ready_pos: &READY_POS + 0.001 + exchange_pos: &EXCHANGE_POS + 1.59 + + ore_lifter: + bin_mid_pos: &BIN_MID_POS + 0.1 + bin_down_pos: &BIN_DOWN_POS + 0.001 + steps_list: ###################### SMALL_ISLAND ####################### SMALL_ISLAND_DEMO: @@ -427,6 +439,12 @@ steps_list: - step: "GIMBAL_READY" gimbal: <<: *SIDE_POS + - step: "ORE_ROTATOR_READY" + ore_rotator: + target: *READY_POS + - step: "ORE_LIFTER_READY" + ore_lifter: + target: *BIN_MID_POS - step: "SI_ARM_READY" arm: joints: [ *JOINT1_READY_POSITION, *JOINT2_BACK_POSITION, *JOINT3_READY_POSITION, *JOINT4_R_POSITION, *JOINT5_MID_POSITION, *JOINT6_MID_POSITION, *JOINT7_MID_POSITION ] @@ -467,6 +485,13 @@ steps_list: tolerance: <<: *SMALL_TOLERANCE - step: "STORE_STONE" + arm: + joints: [ *JOINT1_READY_STORE_POSITION, *JOINT2_STORE_STONE_POSITION, *JOINT3_STORE_STONE_POSITION, *JOINT4_R_POSITION, *JOINT5_MID_POSITION, *JOINT6_UP_POSITION, *JOINT7_MID_POSITION ] + common: + <<: *SLOWLY + tolerance: + <<: *SMALL_TOLERANCE + - step: "JOINT5" arm: joints: [ *JOINT1_READY_STORE_POSITION, *JOINT2_STORE_STONE_POSITION, *JOINT3_STORE_STONE_POSITION, *JOINT4_R_POSITION, *JOINT5_R90_POSITION, *JOINT6_UP_POSITION, *JOINT7_MID_POSITION ] common: @@ -483,12 +508,21 @@ steps_list: <<: *NORMALLY tolerance: <<: *NORMAL_TOLERANCE + - step: "ORE_LIFTER_DOWN" + ore_lifter: + target: *BIN_DOWN_POS ####FIRST_STONE### TWO_STONE_SMALL_ISLAND: - step: "GIMBAL_READY" gimbal: <<: *SIDE_POS + - step: "ORE_ROTATOR_READY" + ore_rotator: + target: *READY_POS + - step: "ORE_LIFTER_READY" + ore_lifter: + target: *BIN_MID_POS - step: "SI_ARM_READY" arm: joints: [ *JOINT1_READY_POSITION, *JOINT2_BACK_POSITION, *JOINT3_READY_POSITION, *JOINT4_R_POSITION, *JOINT5_MID_POSITION, *JOINT6_MID_POSITION, *JOINT7_MID_POSITION ] @@ -529,6 +563,13 @@ steps_list: tolerance: <<: *SMALL_TOLERANCE - step: "STORE_STONE" + arm: + joints: [ *JOINT1_READY_STORE_POSITION, *JOINT2_STORE_STONE_POSITION, *JOINT3_STORE_STONE_POSITION, *JOINT4_R_POSITION, *JOINT5_MID_POSITION, *JOINT6_UP_POSITION, *JOINT7_MID_POSITION ] + common: + <<: *SLOWLY + tolerance: + <<: *SMALL_TOLERANCE + - step: "JOINT5" arm: joints: [ *JOINT1_READY_STORE_POSITION, *JOINT2_STORE_STONE_POSITION, *JOINT3_STORE_STONE_POSITION, *JOINT4_R_POSITION, *JOINT5_R90_POSITION, *JOINT6_UP_POSITION, *JOINT7_MID_POSITION ] common: @@ -545,6 +586,9 @@ steps_list: <<: *NORMALLY tolerance: <<: *NORMAL_TOLERANCE + - step: "ORE_LIFTER_DOWN" + ore_lifter: + target: *BIN_DOWN_POS ###SECOND_STONE### TWO_STONE_SMALL_ISLAND00: - step: "SI_ARM_READY" @@ -587,6 +631,13 @@ steps_list: tolerance: <<: *SMALL_TOLERANCE - step: "STORE_STONE" + arm: + joints: [ *JOINT1_READY_STORE_POSITION, *JOINT2_STORE_STONE_POSITION, *JOINT3_STORE_STONE_POSITION, *JOINT4_R_POSITION, *JOINT5_MID_POSITION, *JOINT6_UP_POSITION, *JOINT7_MID_POSITION ] + common: + <<: *NORMALLY + tolerance: + <<: *SMALL_TOLERANCE + - step: "JOINT5" arm: joints: [ *JOINT1_READY_STORE_POSITION, *JOINT2_STORE_STONE_POSITION, *JOINT3_STORE_STONE_POSITION, *JOINT4_R_POSITION, *JOINT5_R90_POSITION, *JOINT6_UP_POSITION, *JOINT7_MID_POSITION ] common: From 460d02020c6c0eb82d9d75fd256deebadf8577df Mon Sep 17 00:00:00 2001 From: 2194555 <3631676002@qq.com> Date: Tue, 26 Mar 2024 22:54:37 +0800 Subject: [PATCH 09/25] Update. --- engineer_arm_config/config/engineer.srdf | 2 ++ engineer_middleware/include/engineer_middleware/step.h | 2 +- 2 files changed, 3 insertions(+), 1 deletion(-) diff --git a/engineer_arm_config/config/engineer.srdf b/engineer_arm_config/config/engineer.srdf index b4dde6c..dd6c64d 100644 --- a/engineer_arm_config/config/engineer.srdf +++ b/engineer_arm_config/config/engineer.srdf @@ -50,4 +50,6 @@ + + diff --git a/engineer_middleware/include/engineer_middleware/step.h b/engineer_middleware/include/engineer_middleware/step.h index 24fdc4e..f02b4f3 100644 --- a/engineer_middleware/include/engineer_middleware/step.h +++ b/engineer_middleware/include/engineer_middleware/step.h @@ -84,7 +84,7 @@ class Step if (step["scene_name"] == it->first) planning_scene_ = new PlanningScene(it->second, arm_group); } - if (step.hasMember("ore_rotater")) + if (step.hasMember("ore_rotator")) ore_rotate_motion_ = new JointPointMotion(step["ore_rotator"], ore_rotate_pub); if (step.hasMember("ore_lifter")) ore_lift_motion_ = new JointPointMotion(step["ore_lifter"], ore_lift_pub); From 66be75e03a738f3196e886a7d906cb5f2adc901d Mon Sep 17 00:00:00 2001 From: 2194555 <3631676002@qq.com> Date: Wed, 27 Mar 2024 20:51:21 +0800 Subject: [PATCH 10/25] Add get stone from ore bin. --- engineer_middleware/config/steps_list.yaml | 89 +++++++++++++++++++++- 1 file changed, 88 insertions(+), 1 deletion(-) diff --git a/engineer_middleware/config/steps_list.yaml b/engineer_middleware/config/steps_list.yaml index 850daae..b657f66 100644 --- a/engineer_middleware/config/steps_list.yaml +++ b/engineer_middleware/config/steps_list.yaml @@ -33,6 +33,8 @@ common: 0.312 ready_store: &JOINT1_READY_STORE_POSITION 0.162 + bin_get_stone: &JOINT1_BIN_GET_STONE + 0.142 home: zero_stone_position: &JOINT1_ZERO_STONE_POSITION 0.08 @@ -59,6 +61,8 @@ common: 0.02 ready_second_position: &JOINT2_READY_SECOND_POSITION 0.142 + bin_get_stone: &JOINT2_BIN_GET_STONE + 0.087 joint3: mechanical: @@ -78,7 +82,8 @@ common: -0.018 store_stone_position: &JOINT3_STORE_STONE_POSITION 0.30 - + bin_get_stone: &JOINT3_BIN_GET_STONE + 0.258 joint4: mechanical: @@ -98,6 +103,9 @@ common: -1.54 right_90_position: &JOINT5_R90_POSITION 1.59 + bin_get_stone: &JOINT5_BIN_GET_STONE + 1.679 + joint6: mechanical: mid_position: &JOINT6_MID_POSITION @@ -108,6 +116,8 @@ common: 1.569 ready_second_position: &JOINT6_READY_SECOND_POSITION 0.694 + bin_get_stone: &JOINT6_BIN_GET_STONE + -1.611 joint7: mechanical: mid_position: &JOINT7_MID_POSITION @@ -206,6 +216,8 @@ common: 1.59 ore_lifter: + bin_up_pos: &BIN_UP_POS + 0.2 bin_mid_pos: &BIN_MID_POS 0.1 bin_down_pos: &BIN_DOWN_POS @@ -654,6 +666,81 @@ steps_list: <<: *NORMALLY tolerance: <<: *NORMAL_TOLERANCE + + DOWN_STONE_FROM_BIN: + - step: "ORE_ROTATOR_READY" + ore_rotator: + target: *EXCHANGE_POS + - step: "READY_TO_GET" + arm: + joints: [ *JOINT1_BIN_GET_STONE, *JOINT2_BIN_GET_STONE, *JOINT3_BIN_GET_STONE, *JOINT4_R_POSITION, *JOINT5_BIN_GET_STONE, *JOINT6_BIN_GET_STONE, *JOINT7_MID_POSITION ] + common: + <<: *NORMALLY + tolerance: + <<: *NORMAL_TOLERANCE + - step: "OPEN_GRIPPER" + gripper: + <<: *OPEN_GRIPPER + - step: "ORE_LIFTER_UP" + ore_lifter: + target: *BIN_UP_POS + - step: "JOINT2_BACK_GET_STONE" + arm: + joints: [ "KEEP", *JOINT2_BACK_POSITION, "KEEP", "KEEP", "KEEP", "KEEP", "KEEP" ] + common: + <<: *NORMALLY + tolerance: + <<: *NORMAL_TOLERANCE + - step: "JOINT1_UP" + arm: + joints: [ *JOINT1_UP_POSITION, "KEEP", "KEEP", "KEEP", "KEEP", "KEEP", "KEEP" ] + common: + <<: *NORMALLY + tolerance: + <<: *NORMAL_TOLERANCE + - step: "JOINT5_LIFT_STONE" + arm: + joints: [ "KEEP", "KEEP", "KEEP", "KEEP", *JOINT5_MID_POSITION, "KEEP", "KEEP" ] + common: + <<: *NORMALLY + tolerance: + <<: *NORMAL_TOLERANCE + + UP_STONE_FROM_BIN: + - step: "ORE_ROTATOR_READY" + ore_rotator: + target: *EXCHANGE_POS + - step: "READY_TO_GET" + arm: + joints: [ *JOINT1_BIN_GET_STONE, *JOINT2_BIN_GET_STONE, *JOINT3_BIN_GET_STONE, *JOINT4_R_POSITION, *JOINT5_BIN_GET_STONE, *JOINT6_BIN_GET_STONE, *JOINT7_MID_POSITION ] + common: + <<: *NORMALLY + tolerance: + <<: *NORMAL_TOLERANCE + - step: "OPEN_GRIPPER" + gripper: + <<: *OPEN_GRIPPER + - step: "JOINT2_BACK_GET_STONE" + arm: + joints: [ "KEEP", *JOINT2_BACK_POSITION, "KEEP", "KEEP", "KEEP", "KEEP", "KEEP" ] + common: + <<: *NORMALLY + tolerance: + <<: *NORMAL_TOLERANCE + - step: "JOINT1_UP" + arm: + joints: [ *JOINT1_UP_POSITION, "KEEP", "KEEP", "KEEP", "KEEP", "KEEP", "KEEP" ] + common: + <<: *NORMALLY + tolerance: + <<: *NORMAL_TOLERANCE + - step: "JOINT5_LIFT_STONE" + arm: + joints: [ "KEEP", "KEEP", "KEEP", "KEEP", *JOINT5_MID_POSITION, "KEEP", "KEEP" ] + common: + <<: *NORMALLY + tolerance: + <<: *NORMAL_TOLERANCE #################BIG_ISLAND############# # ONE_STONE_BIG_ISLAND： # - gimbal: From d19cf68faedfec8f06fac59b4d32f15d27e41a30 Mon Sep 17 00:00:00 2001 From: 2194555 <3631676002@qq.com> Date: Thu, 28 Mar 2024 00:28:03 +0800 Subject: [PATCH 11/25] Update with tested. --- engineer_middleware/config/steps_list.yaml | 16 +++++++++------- 1 file changed, 9 insertions(+), 7 deletions(-) diff --git a/engineer_middleware/config/steps_list.yaml b/engineer_middleware/config/steps_list.yaml index b657f66..4e2bf1a 100644 --- a/engineer_middleware/config/steps_list.yaml +++ b/engineer_middleware/config/steps_list.yaml @@ -63,6 +63,8 @@ common: 0.142 bin_get_stone: &JOINT2_BIN_GET_STONE 0.087 + back_bin_position: &JOINT2_BACK_BIN_POSITION + 0.001 joint3: mechanical: @@ -671,6 +673,9 @@ steps_list: - step: "ORE_ROTATOR_READY" ore_rotator: target: *EXCHANGE_POS + - step: "ORE_LIFTER_UP" + ore_lifter: + target: *BIN_UP_POS - step: "READY_TO_GET" arm: joints: [ *JOINT1_BIN_GET_STONE, *JOINT2_BIN_GET_STONE, *JOINT3_BIN_GET_STONE, *JOINT4_R_POSITION, *JOINT5_BIN_GET_STONE, *JOINT6_BIN_GET_STONE, *JOINT7_MID_POSITION ] @@ -681,14 +686,11 @@ steps_list: - step: "OPEN_GRIPPER" gripper: <<: *OPEN_GRIPPER - - step: "ORE_LIFTER_UP" - ore_lifter: - target: *BIN_UP_POS - step: "JOINT2_BACK_GET_STONE" arm: - joints: [ "KEEP", *JOINT2_BACK_POSITION, "KEEP", "KEEP", "KEEP", "KEEP", "KEEP" ] + joints: [ "KEEP", *JOINT2_BACK_BIN_POSITION, "KEEP", "KEEP", "KEEP", "KEEP", "KEEP" ] common: - <<: *NORMALLY + <<: *SLOWLY tolerance: <<: *NORMAL_TOLERANCE - step: "JOINT1_UP" @@ -722,9 +724,9 @@ steps_list: <<: *OPEN_GRIPPER - step: "JOINT2_BACK_GET_STONE" arm: - joints: [ "KEEP", *JOINT2_BACK_POSITION, "KEEP", "KEEP", "KEEP", "KEEP", "KEEP" ] + joints: [ "KEEP", *JOINT2_BACK_BIN_POSITION, "KEEP", "KEEP", "KEEP", "KEEP", "KEEP" ] common: - <<: *NORMALLY + <<: *SLOWLY tolerance: <<: *NORMAL_TOLERANCE - step: "JOINT1_UP" From 708299b14a944fef37d10c9990e5288e3da8f660 Mon Sep 17 00:00:00 2001 From: 2194555 <3631676002@qq.com> Date: Mon, 1 Apr 2024 21:51:55 +0800 Subject: [PATCH 12/25] Add big island. --- engineer_middleware/config/steps_list.yaml | 138 +++++++++++++-------- 1 file changed, 84 insertions(+), 54 deletions(-) diff --git a/engineer_middleware/config/steps_list.yaml b/engineer_middleware/config/steps_list.yaml index 4e2bf1a..f0a53b9 100644 --- a/engineer_middleware/config/steps_list.yaml +++ b/engineer_middleware/config/steps_list.yaml @@ -35,6 +35,8 @@ common: 0.162 bin_get_stone: &JOINT1_BIN_GET_STONE 0.142 + big_ready: &JOINT1_BIG_READY + 0.006 home: zero_stone_position: &JOINT1_ZERO_STONE_POSITION 0.08 @@ -65,6 +67,8 @@ common: 0.087 back_bin_position: &JOINT2_BACK_BIN_POSITION 0.001 + big_ready: &JOINT2_BIG_READY + 0.009 joint3: mechanical: @@ -86,6 +90,8 @@ common: 0.30 bin_get_stone: &JOINT3_BIN_GET_STONE 0.258 + big_ready: &JOINT3_BIG_READY + 0.071 joint4: mechanical: @@ -107,6 +113,8 @@ common: 1.59 bin_get_stone: &JOINT5_BIN_GET_STONE 1.679 + big_ready: &JOINT5_BIG_READY + -0.017 joint6: mechanical: @@ -120,6 +128,9 @@ common: 0.694 bin_get_stone: &JOINT6_BIN_GET_STONE -1.611 + big_ready: &JOINT6_BIG_READY + 0.014 + joint7: mechanical: mid_position: &JOINT7_MID_POSITION @@ -128,6 +139,9 @@ common: -1.59 right_90_position: &JOINT7_R90_POSITION 1.57 + big_ready: &JOINT7_BIG_READY + -0.013 + gimbal: side_pos: &SIDE_POS frame: gimbal_lifter @@ -744,57 +758,73 @@ steps_list: tolerance: <<: *NORMAL_TOLERANCE #################BIG_ISLAND############# -# ONE_STONE_BIG_ISLAND： -# - gimbal: -# <<: *NORMALLY -# - step: "BI_ARM_READY" -# arm: -# joints: [ ] -# common: -# <<: *NORMALLY -# tolerance: -# <<: *NORMAL_TOLERANCE -# - step: "OPEN_GRIPPER" -# gripper: -# <<: *OPEN_GRIPPER -# - step: "BI_GET_STONE" -# arm: -# joints: [ ] -# common: -# <<: *NORMALLY -# tolerance: -# <<: *NORMAL_TOLERANCE -# - step: "PULL_OUT_STONE" -# arm: -# joints: [ ] -# common: -# <<: *NORMALLY -# tolerance: -# <<: *NORMAL_TOLERANCE -# - step: "BI_ARM_UP_STONE" #########joint6抬起，无需转动joint7####### -# arm: -# joints: [ ] -# common: -# <<: *NORMALLY -# tolerance: -# <<: *NORMAL_TOLERANCE -# - step: "ORE_LIFTER_UP" -# ore_lifter: -# -# - step: "STORE_STONE" -# arm: -# joints: [ ] -# common: -# <<: *NORMALLY -# tolerance: -# <<: *NORMAL_TOLERANCE -# - step: "CLOSE_GRIPPER" -# gripper: -# <<: *CLOSE_GRIPPER -# - step: "ARM_RESET" -# arm: -# joints: [ ] -# common: -# <<: *NORMALLY -# tolerance: -# <<: *NORMAL_TOLERANCE \ No newline at end of file + MID_BIG_ISLAND: + - step: "MID_ARM_READY" + arm: + joints: [ *JOINT1_BIG_READY, *JOINT2_BIG_READY, *JOINT3_BIG_READY, *JOINT4_R_POSITION, *JOINT5_BIG_READY, *JOINT6_BIG_READY, *JOINT7_BIG_READY ] + common: + <<: *SLOWLY + tolerance: + <<: *NORMAL_TOLERANCE + - step: "OPEN_GRIPPER" + gripper: + <<: *OPEN_GRIPPER + - step: "GET_MID_STONE" + arm: + joints: [ "KEEP", "KEEP", -0.281, "KEEP", "KEEP", "KEEP", "KEEP" ] + common: + <<: *SLOWLY + tolerance: + <<: *NORMAL_TOLERANCE + - step: "JOINT1_MICRO_UP" + arm: + joints: [ 0.071, "KEEP", "KEEP", "KEEP", "KEEP", -0.119, "KEEP" ] + common: + <<: *SLOWLY + tolerance: + <<: *NORMAL_TOLERANCE + - step: "PULL_OUT_STONE" + arm: + joints: [ "KEEP", "KEEP", 0.188, "KEEP", "KEEP", "KEEP", "KEEP" ] + common: + <<: *SLOWLY + tolerance: + <<: *NORMAL_TOLERANCE + - step: "JOINT1_UP" + arm: + joints: [ 0.320, "KEEP", "KEEP", "KEEP", "KEEP", "KEEP", "KEEP" ] + common: + <<: *SLOWLY + tolerance: + <<: *NORMAL_TOLERANCE + - step: "ARM_UP_STONE" + arm: + joints: [ "KEEP", 0.142, 0.289, "KEEP", "KEEP", -1.519, "KEEP" ] + common: + <<: *SLOWLY + tolerance: + <<: *NORMAL_TOLERANCE + - step: "READY_TO_STORE" + arm: + joints: [ 0.155, 0.030, "KEEP", "KEEP", "KEEP", "KEEP", "KEEP" ] + common: + <<: *SLOWLY + tolerance: + <<: *NORMAL_TOLERANCE + - step: "STORE_STONE" + arm: + joints: [ "KEEP", "KEEP", "KEEP", "KEEP", 1.617, "KEEP", "KEEP" ] + common: + <<: *SLOWLY + tolerance: + <<: *NORMAL_TOLERANCE + - step: "CLOSE_GRIPPER" + gripper: + <<: *CLOSE_GRIPPER + - step: "ARM_RESET" + arm: + joints: [ 0.006, 0.009, 0.071, -1.495, -0.017, 0.014, -0.013 ] + common: + <<: *SLOWLY + tolerance: + <<: *NORMAL_TOLERANCE \ No newline at end of file From a401cf01c2d9b36f17b139791f77bb72d7ea260b Mon Sep 17 00:00:00 2001 From: 2194555 <3631676002@qq.com> Date: Mon, 1 Apr 2024 22:48:32 +0800 Subject: [PATCH 13/25] Add and modify parameters. --- engineer_middleware/config/steps_list.yaml | 162 ++++++++++++--------- 1 file changed, 97 insertions(+), 65 deletions(-) diff --git a/engineer_middleware/config/steps_list.yaml b/engineer_middleware/config/steps_list.yaml index f0a53b9..2e84ba0 100644 --- a/engineer_middleware/config/steps_list.yaml +++ b/engineer_middleware/config/steps_list.yaml @@ -27,16 +27,22 @@ common: mechanical: down_position: &JOINT1_DOWN_POSITION 0.015 - ready_position: &JOINT1_READY_POSITION + small_ready: &JOINT1_SMALL_READY 0.15 - up_position: &JOINT1_UP_POSITION + up_pos: &JOINT1_UP_POS 0.312 - ready_store: &JOINT1_READY_STORE_POSITION + small_ready_store: &JOINT1_SMALL_READY_STORE 0.162 bin_get_stone: &JOINT1_BIN_GET_STONE 0.142 big_ready: &JOINT1_BIG_READY 0.006 + big_adjust_mid: &JOINT1_BIG_ADJUST_MID + 0.071 + big_up_pos: &JOINT1_BIG_UP_POS + 0.320 + big_ready_store: &JOINT1_BIG_READY_STORE + 0.155 home: zero_stone_position: &JOINT1_ZERO_STONE_POSITION 0.08 @@ -57,11 +63,11 @@ common: 0.232 exchange_position: &JOINT2_EXCHANGE_POSITION 0.07 - lift_stone_position: &JOINT2_LIFT_STONE_POSITION + small_arm_up: &JOINT2_SMALL_ARM_UP 0.085 - store_stone_position: &JOINT2_STORE_STONE_POSITION + small_store_stone: &JOINT2_SMALL_STORE_STONE 0.02 - ready_second_position: &JOINT2_READY_SECOND_POSITION + small_ready_second: &JOINT2_SMALL_READY_SECOND 0.142 bin_get_stone: &JOINT2_BIN_GET_STONE 0.087 @@ -69,6 +75,10 @@ common: 0.001 big_ready: &JOINT2_BIG_READY 0.009 + big_arm_up: &JOINT2_BIG_ARM_UP + 0.142 + big_ready_store: &JOINT2_BIG_READY_STORE + 0.030 joint3: mechanical: @@ -78,20 +88,26 @@ common: 0.0001 right_position: &JOINT3_R_POSITION -0.275 - ready_position: &JOINT3_READY_POSITION + small_ready: &JOINT3_SMALL_READY 0.098 - ready_second_position: &JOINT3_READY_SECOND_POSITION + small_ready_second: &JOINT3_SMALL_READY_SECOND 0.03 - get_stone_position: &JOINT3_GET_STONE_POSITION + small_get_stone: &JOINT3_SMALL_GET_STONE 0.034 - get_second_position: &JOINT3_GET_SECOND_POSITION + small_get_second: &JOINT3_SMALL_GET_SECOND -0.018 - store_stone_position: &JOINT3_STORE_STONE_POSITION + small_store_stone: &JOINT3_SMALL_STORE_STONE 0.30 bin_get_stone: &JOINT3_BIN_GET_STONE 0.258 big_ready: &JOINT3_BIG_READY 0.071 + get_big_mid: &JOINT3_GET_BIG_MID + -0.281 + big_pull_out: &JOINT3_BIG_PULL_OUT + 0.188 + big_arm_up: &JOINT3_BIG_ARM_UP + 0.289 joint4: mechanical: @@ -100,8 +116,8 @@ common: mid_position: &JOINT4_MID_POSITION 0.001 right_position: &JOINT4_R_POSITION - -1.49 - ready_second_position: &JOINT4_READY_SECOND_POSITION + -1.495 + small_ready_second: &JOINT4_SMALL_READY_SECOND -0.777 joint5: mechanical: @@ -115,6 +131,8 @@ common: 1.679 big_ready: &JOINT5_BIG_READY -0.017 + big_store: &JOINT5_BIG_STORE + 1.617 joint6: mechanical: @@ -124,12 +142,16 @@ common: -1.569 down_position: &JOINT6_DOWN_POSITION 1.569 - ready_second_position: &JOINT6_READY_SECOND_POSITION + small_ready_second: &JOINT6_SMALL_READY_SECOND 0.694 bin_get_stone: &JOINT6_BIN_GET_STONE -1.611 big_ready: &JOINT6_BIG_READY 0.014 + big_adjust_mid: &JOINT6_BIG_ADJUST_MID + -0.119 + big_arm_up: &JOINT6_BIG_ARM_UP + -1.519 joint7: mechanical: @@ -139,7 +161,7 @@ common: -1.59 right_90_position: &JOINT7_R90_POSITION 1.57 - big_ready: &JOINT7_BIG_READY + big_pos: &JOINT7_BIG_POS -0.013 gimbal: @@ -473,9 +495,9 @@ steps_list: - step: "ORE_LIFTER_READY" ore_lifter: target: *BIN_MID_POS - - step: "SI_ARM_READY" + - step: "SMALL_ARM_READY" arm: - joints: [ *JOINT1_READY_POSITION, *JOINT2_BACK_POSITION, *JOINT3_READY_POSITION, *JOINT4_R_POSITION, *JOINT5_MID_POSITION, *JOINT6_MID_POSITION, *JOINT7_MID_POSITION ] + joints: [ *JOINT1_SMALL_READY, *JOINT2_BACK_POSITION, *JOINT3_SMALL_READY, *JOINT4_R_POSITION, *JOINT5_MID_POSITION, *JOINT6_MID_POSITION, *JOINT7_MID_POSITION ] common: <<: *NORMALLY tolerance: @@ -484,44 +506,44 @@ steps_list: gripper: <<: *OPEN_GRIPPER ONE_STONE_SMALL_ISLAND0: - - step: "SI_GET_STONE" + - step: "SMALL_GET_STONE" arm: - joints: [ *JOINT1_READY_POSITION, *JOINT2_BACK_POSITION, *JOINT3_GET_STONE_POSITION, *JOINT4_R_POSITION, *JOINT5_MID_POSITION, *JOINT6_MID_POSITION, *JOINT7_MID_POSITION ] + joints: [ *JOINT1_SMALL_READY, *JOINT2_BACK_POSITION, *JOINT3_SMALL_GET_STONE, *JOINT4_R_POSITION, *JOINT5_MID_POSITION, *JOINT6_MID_POSITION, *JOINT7_MID_POSITION ] common: <<: *NORMALLY tolerance: <<: *NORMAL_TOLERANCE - step: "JOINT1_UP" arm: - joints: [ *JOINT1_UP_POSITION, "KEEP", "KEEP", "KEEP", "KEEP", "KEEP", "KEEP"] + joints: [ *JOINT1_UP_POS, "KEEP", "KEEP", "KEEP", "KEEP", "KEEP", "KEEP"] common: <<: *NORMALLY tolerance: <<: *NORMAL_TOLERANCE - - step: "SI_ARM_LIFT_STONE" + - step: "SMALL_ARM_UP_STONE" arm: - joints: [ *JOINT1_UP_POSITION, *JOINT2_LIFT_STONE_POSITION, *JOINT3_GET_STONE_POSITION, *JOINT4_R_POSITION, *JOINT5_MID_POSITION, *JOINT6_UP_POSITION, *JOINT7_MID_POSITION ] + joints: [ *JOINT1_UP_POS, *JOINT2_SMALL_ARM_UP, *JOINT3_SMALL_GET_STONE, *JOINT4_R_POSITION, *JOINT5_MID_POSITION, *JOINT6_UP_POSITION, *JOINT7_MID_POSITION ] common: <<: *SLOWLY tolerance: <<: *NORMAL_TOLERANCE - step: "READY_TO_STORE_STONE" arm: - joints: [ *JOINT1_READY_STORE_POSITION, *JOINT2_LIFT_STONE_POSITION, *JOINT3_STORE_STONE_POSITION, *JOINT4_R_POSITION, *JOINT5_MID_POSITION, *JOINT6_UP_POSITION, *JOINT7_MID_POSITION ] + joints: [ *JOINT1_SMALL_READY_STORE, *JOINT2_SMALL_ARM_UP, *JOINT3_SMALL_STORE_STONE, *JOINT4_R_POSITION, *JOINT5_MID_POSITION, *JOINT6_UP_POSITION, *JOINT7_MID_POSITION ] common: <<: *NORMALLY tolerance: <<: *SMALL_TOLERANCE - step: "STORE_STONE" arm: - joints: [ *JOINT1_READY_STORE_POSITION, *JOINT2_STORE_STONE_POSITION, *JOINT3_STORE_STONE_POSITION, *JOINT4_R_POSITION, *JOINT5_MID_POSITION, *JOINT6_UP_POSITION, *JOINT7_MID_POSITION ] + joints: [ *JOINT1_SMALL_READY_STORE, *JOINT2_SMALL_STORE_STONE, *JOINT3_SMALL_STORE_STONE, *JOINT4_R_POSITION, *JOINT5_MID_POSITION, *JOINT6_UP_POSITION, *JOINT7_MID_POSITION ] common: <<: *SLOWLY tolerance: <<: *SMALL_TOLERANCE - - step: "JOINT5" + - step: "JOINT5_TURN" arm: - joints: [ *JOINT1_READY_STORE_POSITION, *JOINT2_STORE_STONE_POSITION, *JOINT3_STORE_STONE_POSITION, *JOINT4_R_POSITION, *JOINT5_R90_POSITION, *JOINT6_UP_POSITION, *JOINT7_MID_POSITION ] + joints: [ *JOINT1_SMALL_READY_STORE, *JOINT2_SMALL_STORE_STONE, *JOINT3_SMALL_STORE_STONE, *JOINT4_R_POSITION, *JOINT5_R90_POSITION, *JOINT6_UP_POSITION, *JOINT7_MID_POSITION ] common: <<: *SLOWLY tolerance: @@ -531,7 +553,7 @@ steps_list: <<: *CLOSE_GRIPPER - step: "ARM_RESET" arm: - joints: [ *JOINT1_READY_STORE_POSITION, *JOINT2_STORE_STONE_POSITION, *JOINT3_L_POSITION, *JOINT4_R_POSITION, *JOINT5_MID_POSITION, *JOINT6_MID_POSITION, *JOINT7_MID_POSITION ] + joints: [ *JOINT1_SMALL_READY_STORE, *JOINT2_SMALL_STORE_STONE, *JOINT3_L_POSITION, *JOINT4_R_POSITION, *JOINT5_MID_POSITION, *JOINT6_MID_POSITION, *JOINT7_MID_POSITION ] common: <<: *NORMALLY tolerance: @@ -551,9 +573,9 @@ steps_list: - step: "ORE_LIFTER_READY" ore_lifter: target: *BIN_MID_POS - - step: "SI_ARM_READY" + - step: "SMALL_ARM_READY" arm: - joints: [ *JOINT1_READY_POSITION, *JOINT2_BACK_POSITION, *JOINT3_READY_POSITION, *JOINT4_R_POSITION, *JOINT5_MID_POSITION, *JOINT6_MID_POSITION, *JOINT7_MID_POSITION ] + joints: [ *JOINT1_SMALL_READY, *JOINT2_BACK_POSITION, *JOINT3_SMALL_READY, *JOINT4_R_POSITION, *JOINT5_MID_POSITION, *JOINT6_MID_POSITION, *JOINT7_MID_POSITION ] common: <<: *NORMALLY tolerance: @@ -562,44 +584,44 @@ steps_list: gripper: <<: *OPEN_GRIPPER TWO_STONE_SMALL_ISLAND0: - - step: "SI_GET_STONE" + - step: "SMALL_GET_STONE" arm: - joints: [ *JOINT1_READY_POSITION, *JOINT2_BACK_POSITION, *JOINT3_GET_STONE_POSITION, *JOINT4_R_POSITION, *JOINT5_MID_POSITION, *JOINT6_MID_POSITION, *JOINT7_MID_POSITION ] + joints: [ *JOINT1_SMALL_READY, *JOINT2_BACK_POSITION, *JOINT3_SMALL_GET_STONE, *JOINT4_R_POSITION, *JOINT5_MID_POSITION, *JOINT6_MID_POSITION, *JOINT7_MID_POSITION ] common: <<: *NORMALLY tolerance: <<: *NORMAL_TOLERANCE - step: "JOINT1_UP" arm: - joints: [ *JOINT1_UP_POSITION, "KEEP", "KEEP", "KEEP", "KEEP", "KEEP", "KEEP"] + joints: [ *JOINT1_UP_POS, "KEEP", "KEEP", "KEEP", "KEEP", "KEEP", "KEEP"] common: <<: *NORMALLY tolerance: <<: *NORMAL_TOLERANCE - - step: "SI_ARM_LIFT_STONE" + - step: "SMALL_ARM_UP_STONE" arm: - joints: [ *JOINT1_UP_POSITION, *JOINT2_LIFT_STONE_POSITION, *JOINT3_GET_STONE_POSITION, *JOINT4_R_POSITION, *JOINT5_MID_POSITION, *JOINT6_UP_POSITION, *JOINT7_MID_POSITION ] + joints: [ *JOINT1_UP_POS, *JOINT2_SMALL_ARM_UP, *JOINT3_SMALL_GET_STONE, *JOINT4_R_POSITION, *JOINT5_MID_POSITION, *JOINT6_UP_POSITION, *JOINT7_MID_POSITION ] common: <<: *SLOWLY tolerance: <<: *NORMAL_TOLERANCE - step: "READY_TO_STORE_STONE" arm: - joints: [ *JOINT1_READY_STORE_POSITION, *JOINT2_LIFT_STONE_POSITION, *JOINT3_STORE_STONE_POSITION, *JOINT4_R_POSITION, *JOINT5_MID_POSITION, *JOINT6_UP_POSITION, *JOINT7_MID_POSITION ] + joints: [ *JOINT1_SMALL_READY_STORE, *JOINT2_SMALL_ARM_UP, *JOINT3_SMALL_STORE_STONE, *JOINT4_R_POSITION, *JOINT5_MID_POSITION, *JOINT6_UP_POSITION, *JOINT7_MID_POSITION ] common: <<: *NORMALLY tolerance: <<: *SMALL_TOLERANCE - step: "STORE_STONE" arm: - joints: [ *JOINT1_READY_STORE_POSITION, *JOINT2_STORE_STONE_POSITION, *JOINT3_STORE_STONE_POSITION, *JOINT4_R_POSITION, *JOINT5_MID_POSITION, *JOINT6_UP_POSITION, *JOINT7_MID_POSITION ] + joints: [ *JOINT1_SMALL_READY_STORE, *JOINT2_SMALL_STORE_STONE, *JOINT3_SMALL_STORE_STONE, *JOINT4_R_POSITION, *JOINT5_MID_POSITION, *JOINT6_UP_POSITION, *JOINT7_MID_POSITION ] common: <<: *SLOWLY tolerance: <<: *SMALL_TOLERANCE - - step: "JOINT5" + - step: "JOINT5_TURN" arm: - joints: [ *JOINT1_READY_STORE_POSITION, *JOINT2_STORE_STONE_POSITION, *JOINT3_STORE_STONE_POSITION, *JOINT4_R_POSITION, *JOINT5_R90_POSITION, *JOINT6_UP_POSITION, *JOINT7_MID_POSITION ] + joints: [ *JOINT1_SMALL_READY_STORE, *JOINT2_SMALL_STORE_STONE, *JOINT3_SMALL_STORE_STONE, *JOINT4_R_POSITION, *JOINT5_R90_POSITION, *JOINT6_UP_POSITION, *JOINT7_MID_POSITION ] common: <<: *SLOWLY tolerance: @@ -609,7 +631,7 @@ steps_list: <<: *CLOSE_GRIPPER - step: "ARM_RESET" arm: - joints: [ *JOINT1_READY_STORE_POSITION, *JOINT2_STORE_STONE_POSITION, *JOINT3_L_POSITION, *JOINT4_R_POSITION, *JOINT5_MID_POSITION, *JOINT6_MID_POSITION, *JOINT7_MID_POSITION ] + joints: [ *JOINT1_SMALL_READY_STORE, *JOINT2_SMALL_STORE_STONE, *JOINT3_L_POSITION, *JOINT4_R_POSITION, *JOINT5_MID_POSITION, *JOINT6_MID_POSITION, *JOINT7_MID_POSITION ] common: <<: *NORMALLY tolerance: @@ -619,9 +641,9 @@ steps_list: target: *BIN_DOWN_POS ###SECOND_STONE### TWO_STONE_SMALL_ISLAND00: - - step: "SI_ARM_READY" + - step: "SMALL_ARM_READY_TWO" arm: - joints: [ *JOINT1_READY_POSITION, *JOINT2_READY_SECOND_POSITION, *JOINT3_READY_SECOND_POSITION, *JOINT4_READY_SECOND_POSITION, *JOINT5_L90_POSITION, *JOINT6_READY_SECOND_POSITION, *JOINT7_R90_POSITION ] + joints: [ *JOINT1_SMALL_READY, *JOINT2_SMALL_READY_SECOND, *JOINT3_SMALL_READY_SECOND, *JOINT4_SMALL_READY_SECOND, *JOINT5_L90_POSITION, *JOINT6_SMALL_READY_SECOND, *JOINT7_R90_POSITION ] common: <<: *NORMALLY tolerance: @@ -630,44 +652,44 @@ steps_list: gripper: <<: *OPEN_GRIPPER TWO_STONE_SMALL_ISLAND000: - - step: "SI_GET_STONE" + - step: "SMALL_GET_STONE_TWO" arm: - joints: [ *JOINT1_READY_POSITION, *JOINT2_READY_SECOND_POSITION, *JOINT3_GET_SECOND_POSITION, *JOINT4_READY_SECOND_POSITION, *JOINT5_L90_POSITION, *JOINT6_READY_SECOND_POSITION, *JOINT7_R90_POSITION ] + joints: [ *JOINT1_SMALL_READY, *JOINT2_SMALL_READY_SECOND, *JOINT3_SMALL_GET_SECOND, *JOINT4_SMALL_READY_SECOND, *JOINT5_L90_POSITION, *JOINT6_SMALL_READY_SECOND, *JOINT7_R90_POSITION ] common: <<: *NORMALLY tolerance: <<: *SMALL_TOLERANCE - step: "JOINT1_UP" arm: - joints: [ *JOINT1_UP_POSITION, "KEEP", "KEEP", "KEEP", "KEEP", "KEEP", "KEEP"] + joints: [ *JOINT1_UP_POS, "KEEP", "KEEP", "KEEP", "KEEP", "KEEP", "KEEP"] common: <<: *NORMALLY tolerance: <<: *NORMAL_TOLERANCE - - step: "SI_ARM_LIFT_STONE" + - step: "SMALL_ARM_LIFT_STONE" arm: - joints: [ *JOINT1_UP_POSITION, *JOINT2_LIFT_STONE_POSITION, *JOINT3_GET_STONE_POSITION, *JOINT4_R_POSITION, *JOINT5_MID_POSITION, *JOINT6_UP_POSITION, *JOINT7_MID_POSITION ] + joints: [ *JOINT1_UP_POS, *JOINT2_SMALL_ARM_UP, *JOINT3_SMALL_GET_STONE, *JOINT4_R_POSITION, *JOINT5_MID_POSITION, *JOINT6_UP_POSITION, *JOINT7_MID_POSITION ] common: <<: *NORMALLY tolerance: <<: *NORMAL_TOLERANCE - step: "READY_TO_STORE_STONE" arm: - joints: [ *JOINT1_READY_STORE_POSITION, *JOINT2_LIFT_STONE_POSITION, *JOINT3_STORE_STONE_POSITION, *JOINT4_R_POSITION, *JOINT5_MID_POSITION, *JOINT6_UP_POSITION, *JOINT7_MID_POSITION ] + joints: [ *JOINT1_SMALL_READY_STORE, *JOINT2_SMALL_ARM_UP, *JOINT3_SMALL_STORE_STONE, *JOINT4_R_POSITION, *JOINT5_MID_POSITION, *JOINT6_UP_POSITION, *JOINT7_MID_POSITION ] common: <<: *NORMALLY tolerance: <<: *SMALL_TOLERANCE - step: "STORE_STONE" arm: - joints: [ *JOINT1_READY_STORE_POSITION, *JOINT2_STORE_STONE_POSITION, *JOINT3_STORE_STONE_POSITION, *JOINT4_R_POSITION, *JOINT5_MID_POSITION, *JOINT6_UP_POSITION, *JOINT7_MID_POSITION ] + joints: [ *JOINT1_SMALL_READY_STORE, *JOINT2_SMALL_STORE_STONE, *JOINT3_SMALL_STORE_STONE, *JOINT4_R_POSITION, *JOINT5_MID_POSITION, *JOINT6_UP_POSITION, *JOINT7_MID_POSITION ] common: <<: *NORMALLY tolerance: <<: *SMALL_TOLERANCE - step: "JOINT5" arm: - joints: [ *JOINT1_READY_STORE_POSITION, *JOINT2_STORE_STONE_POSITION, *JOINT3_STORE_STONE_POSITION, *JOINT4_R_POSITION, *JOINT5_R90_POSITION, *JOINT6_UP_POSITION, *JOINT7_MID_POSITION ] + joints: [ *JOINT1_SMALL_READY_STORE, *JOINT2_SMALL_STORE_STONE, *JOINT3_SMALL_STORE_STONE, *JOINT4_R_POSITION, *JOINT5_R90_POSITION, *JOINT6_UP_POSITION, *JOINT7_MID_POSITION ] common: <<: *NORMALLY tolerance: @@ -677,13 +699,13 @@ steps_list: <<: *CLOSE_GRIPPER - step: "ARM_RESET" arm: - joints: [ *JOINT1_READY_STORE_POSITION, *JOINT2_STORE_STONE_POSITION, *JOINT3_L_POSITION, *JOINT4_R_POSITION, *JOINT5_MID_POSITION, *JOINT6_MID_POSITION, *JOINT7_MID_POSITION ] + joints: [ *JOINT1_SMALL_READY_STORE, *JOINT2_SMALL_STORE_STONE, *JOINT3_L_POSITION, *JOINT4_R_POSITION, *JOINT5_MID_POSITION, *JOINT6_MID_POSITION, *JOINT7_MID_POSITION ] common: <<: *NORMALLY tolerance: <<: *NORMAL_TOLERANCE - DOWN_STONE_FROM_BIN: + GET_DOWN_STONE_BIN: - step: "ORE_ROTATOR_READY" ore_rotator: target: *EXCHANGE_POS @@ -709,7 +731,7 @@ steps_list: <<: *NORMAL_TOLERANCE - step: "JOINT1_UP" arm: - joints: [ *JOINT1_UP_POSITION, "KEEP", "KEEP", "KEEP", "KEEP", "KEEP", "KEEP" ] + joints: [ *JOINT1_UP_POS, "KEEP", "KEEP", "KEEP", "KEEP", "KEEP", "KEEP" ] common: <<: *NORMALLY tolerance: @@ -722,7 +744,7 @@ steps_list: tolerance: <<: *NORMAL_TOLERANCE - UP_STONE_FROM_BIN: + GET_UP_STONE_BIN: - step: "ORE_ROTATOR_READY" ore_rotator: target: *EXCHANGE_POS @@ -745,7 +767,7 @@ steps_list: <<: *NORMAL_TOLERANCE - step: "JOINT1_UP" arm: - joints: [ *JOINT1_UP_POSITION, "KEEP", "KEEP", "KEEP", "KEEP", "KEEP", "KEEP" ] + joints: [ *JOINT1_UP_POS, "KEEP", "KEEP", "KEEP", "KEEP", "KEEP", "KEEP" ] common: <<: *NORMALLY tolerance: @@ -757,63 +779,70 @@ steps_list: <<: *NORMALLY tolerance: <<: *NORMAL_TOLERANCE + #################BIG_ISLAND############# MID_BIG_ISLAND: - step: "MID_ARM_READY" arm: - joints: [ *JOINT1_BIG_READY, *JOINT2_BIG_READY, *JOINT3_BIG_READY, *JOINT4_R_POSITION, *JOINT5_BIG_READY, *JOINT6_BIG_READY, *JOINT7_BIG_READY ] + joints: [ *JOINT1_BIG_READY, *JOINT2_BIG_READY, *JOINT3_BIG_READY, *JOINT4_R_POSITION, *JOINT5_BIG_READY, *JOINT6_BIG_READY, *JOINT7_BIG_POS ] common: <<: *SLOWLY tolerance: <<: *NORMAL_TOLERANCE + - step: "ORE_ROTATOR_READY" + ore_rotator: + target: *READY_POS + - step: "ORE_LIFTER_READY" + ore_lifter: + target: *BIN_MID_POS - step: "OPEN_GRIPPER" gripper: <<: *OPEN_GRIPPER - step: "GET_MID_STONE" arm: - joints: [ "KEEP", "KEEP", -0.281, "KEEP", "KEEP", "KEEP", "KEEP" ] + joints: [ *JOINT1_BIG_READY, *JOINT2_BIG_READY, *JOINT3_GET_BIG_MID, *JOINT4_R_POSITION, *JOINT5_BIG_READY, *JOINT6_BIG_READY, *JOINT7_BIG_POS ] common: <<: *SLOWLY tolerance: <<: *NORMAL_TOLERANCE - - step: "JOINT1_MICRO_UP" + - step: "ADJUST_STONE" arm: - joints: [ 0.071, "KEEP", "KEEP", "KEEP", "KEEP", -0.119, "KEEP" ] + joints: [ *JOINT1_BIG_ADJUST_MID, *JOINT2_BIG_READY, *JOINT3_GET_BIG_MID, *JOINT4_R_POSITION, *JOINT5_BIG_READY, *JOINT6_BIG_ADJUST_MID, *JOINT7_BIG_POS ] common: <<: *SLOWLY tolerance: <<: *NORMAL_TOLERANCE - step: "PULL_OUT_STONE" arm: - joints: [ "KEEP", "KEEP", 0.188, "KEEP", "KEEP", "KEEP", "KEEP" ] + joints: [ "KEEP", "KEEP", *JOINT3_BIG_PULL_OUT, "KEEP", "KEEP", "KEEP", "KEEP" ] common: <<: *SLOWLY tolerance: <<: *NORMAL_TOLERANCE - step: "JOINT1_UP" arm: - joints: [ 0.320, "KEEP", "KEEP", "KEEP", "KEEP", "KEEP", "KEEP" ] + joints: [ *JOINT1_BIG_UP_POS, "KEEP", "KEEP", "KEEP", "KEEP", "KEEP", "KEEP" ] common: <<: *SLOWLY tolerance: <<: *NORMAL_TOLERANCE - step: "ARM_UP_STONE" arm: - joints: [ "KEEP", 0.142, 0.289, "KEEP", "KEEP", -1.519, "KEEP" ] + joints: [ *JOINT1_BIG_UP_POS, *JOINT2_BIG_ARM_UP, *JOINT3_BIG_ARM_UP, *JOINT4_R_POSITION, *JOINT5_BIG_READY, *JOINT6_BIG_ARM_UP, *JOINT7_BIG_POS ] common: <<: *SLOWLY tolerance: <<: *NORMAL_TOLERANCE - step: "READY_TO_STORE" arm: - joints: [ 0.155, 0.030, "KEEP", "KEEP", "KEEP", "KEEP", "KEEP" ] + joints: [ *JOINT1_BIG_READY_STORE, *JOINT2_BIG_READY_STORE, *JOINT3_BIG_ARM_UP, *JOINT4_R_POSITION, *JOINT5_BIG_READY, *JOINT6_BIG_ARM_UP, *JOINT7_BIG_POS ] common: <<: *SLOWLY tolerance: <<: *NORMAL_TOLERANCE - step: "STORE_STONE" arm: - joints: [ "KEEP", "KEEP", "KEEP", "KEEP", 1.617, "KEEP", "KEEP" ] + joints: [ *JOINT1_BIG_READY_STORE, *JOINT2_BIG_READY_STORE, *JOINT3_BIG_ARM_UP, *JOINT4_R_POSITION, *JOINT5_BIG_STORE, *JOINT6_BIG_ARM_UP, *JOINT7_BIG_POS ] common: <<: *SLOWLY tolerance: @@ -823,8 +852,11 @@ steps_list: <<: *CLOSE_GRIPPER - step: "ARM_RESET" arm: - joints: [ 0.006, 0.009, 0.071, -1.495, -0.017, 0.014, -0.013 ] + joints: [ *JOINT1_BIG_READY, *JOINT2_BIG_READY, *JOINT3_GET_BIG_MID, *JOINT4_R_POSITION, *JOINT5_BIG_READY, *JOINT6_BIG_READY, *JOINT7_BIG_POS ] common: <<: *SLOWLY tolerance: - <<: *NORMAL_TOLERANCE \ No newline at end of file + <<: *NORMAL_TOLERANCE + - step: "ORE_LIFTER_DOWN" + ore_lifter: + target: *BIN_DOWN_POS \ No newline at end of file From 2fd93e26b66f179a7e07064ad114c06656fe7499 Mon Sep 17 00:00:00 2001 From: cc0h <7921166012@qq.com> Date: Tue, 2 Apr 2024 20:26:53 +0800 Subject: [PATCH 14/25] disable collision check between joint7 and ore bin rotator. --- engineer_arm_config/config/engineer.srdf | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/engineer_arm_config/config/engineer.srdf b/engineer_arm_config/config/engineer.srdf index dd6c64d..604c9d1 100644 --- a/engineer_arm_config/config/engineer.srdf +++ b/engineer_arm_config/config/engineer.srdf @@ -51,5 +51,5 @@ - + From 12c6436f2475a34ca8c6682b61a0ce5d82c81f46 Mon Sep 17 00:00:00 2001 From: cc0h <7921166012@qq.com> Date: Tue, 2 Apr 2024 20:39:28 +0800 Subject: [PATCH 15/25] optimize step lists. --- engineer_middleware/config/steps_list.yaml | 270 ++++++++++----------- 1 file changed, 133 insertions(+), 137 deletions(-) diff --git a/engineer_middleware/config/steps_list.yaml b/engineer_middleware/config/steps_list.yaml index bf0fc16..525c465 100644 --- a/engineer_middleware/config/steps_list.yaml +++ b/engineer_middleware/config/steps_list.yaml @@ -167,13 +167,10 @@ common: gimbal: side_pos: &SIDE_POS frame: gimbal_lifter - position: [ 0.03 ,3, -0.02 ] - island_pos: &ISLAND_POS + position: [ 0.001 , -3, 0. ] + front_pos: &FRONT_POS frame: gimbal_lifter - position: [ 2, 0.001, 0.001 ] - ground_pos: &GROUND_POS - frame: gimbal_lifter - position: [ 2, 0.001, -0.8 ] + position: [ 2, 0.001, 0. ] gimbal_lifter: button_pos: &BUTTON_POS @@ -200,35 +197,6 @@ common: <<: *NORMAL_TOLERANCE - reversal: - velocity_stop: &VELOCITY_STOP - mode: "VELOCITY" - values: [ 0.,0.,0.,0.,0.,0. ] - velocity_z_out: &VELOCITY_Z_OUT - mode: "VELOCITY" - values: [ 0.,0.,3.,0.,0.,0. ] - velocity_z_out_store: &VELOCITY_Z_OUT_STORE - mode: "VELOCITY" - values: [ 0.,0.,0.2,0.,0.,0. ] - velocity_z_in: &VELOCITY_Z_IN - mode: "VELOCITY" - values: [ 0.,0.,-1.2,0.,-0.01,0. ] - velocity_z_in_quickly: &VELOCITY_Z_IN_QUICKLY - mode: "VELOCITY" - values: [ 0.,0.,-3.0,0.,-0.01,0. ] - velocity_z_in_quickly_with_delay: &VELOCITY_Z_IN_QUICKLY_WITH_DELAY - mode: "VELOCITY" - values: [ 0.,0.,-3.0,0.,-0.01,0. ] - delay: 0.3 - position_z_out: &POSITION_Z_OUT - mode: "POSITION" - values: [ 0.,0.,7.,0.,0.,0. ] - delay: 0.2 - position_z_out_little: &POSITION_Z_OUT_LITTLE - mode: "POSITION" - values: [ 0.,0.,3.,0.,0.,0. ] - delay: 0.4 - chassis_move: chassis_backward_30: &CHASSIS_BACKWARD_25 frame: base_link @@ -265,7 +233,7 @@ steps_list: EXCHANGE_POS: - step: "gimbal look front" gimbal: - <<: *ISLAND_POS + <<: *FRONT_POS - step: "move arm" arm: joints: [*JOINT1_TWO_STONE_POSITION, *JOINT2_EXCHANGE_POSITION, *JOINT3_L_POSITION, *JOINT4_MID_POSITION, *JOINT5_MID_POSITION, *JOINT6_UP_POSITION, *JOINT7_MID_POSITION] @@ -277,7 +245,7 @@ steps_list: GIMBAL_FRONT: - step: "gimbal look front" gimbal: - <<: *ISLAND_POS + <<: *FRONT_POS GIMBAL_LEFT: - step: "gimbal look left" gimbal: @@ -288,7 +256,7 @@ steps_list: HOME_ZERO_STONE: - step: "gimbal look side" gimbal: - <<: *ISLAND_POS + <<: *FRONT_POS - step: "close gripper" gripper: <<: *CLOSE_GRIPPER @@ -299,7 +267,7 @@ steps_list: HOME_ONE_STONE: - step: "gimbal look side" gimbal: - <<: *ISLAND_POS + <<: *FRONT_POS - step: "close gripper" gripper: <<: *CLOSE_GRIPPER @@ -310,7 +278,7 @@ steps_list: HOME_TWO_STONE: - step: "gimbal look side" gimbal: - <<: *ISLAND_POS + <<: *FRONT_POS - step: "close gripper" gripper: <<: *CLOSE_GRIPPER @@ -328,103 +296,7 @@ steps_list: gripper: <<: *CLOSE_GRIPPER - ########## EXCHANGE ############ - OLD_EXCHANGE_SPHERE: - - step: "close to a sphere" - arm: - frame: exchanger - xyz: [ -0.4, 0., -0.25 ] - rpy: [ 0., 0., 0. ] - rpy_rectify: [ 0.01,0.1,0.03 ] - drift_dimensions: [ true, true, true, true, true, true ] - tolerance_position: 0.1 - tolerance_orientation: 0.01 - spacial_shape: SPHERE - radius: 0.5 - point_resolution: 0.5 - max_planning_times: 1 - common: - <<: *QUICKLY - - EXCHANGE_DOWN: - - step: "test new exchange" - arm: - frame: exchanger - is_refer_planning_frame: true - xyz: [ -0.3, 0.2, -0.2 ] - rpy: [ 0., -1.5707963, 0. ] - drift_dimensions: [ true, true, true, true, true, true ] - tolerance_position: 0.01 - tolerance_orientation: 0.001 - spacial_shape: SPHERE - radius: 0.3 - point_resolution: 0.5 - max_planning_times: 1 - common: - <<: *NORMALLY - - TEST_ORI: - - step: "ori test" - arm: - frame: exchanger - rpy: [ 0., -1.5707963, 0. ] - tolerance_orientation: 0.001 - - TEST_ORI2: - - step: "ori test2" - arm: - frame: exchanger - rpy: [ 0., -1.5707963, 0. ] - xyz: [ -0.3, 0.2, -0.2 ] - tolerance_position: 0.01 - tolerance_orientation: 0.001 - - EXCHANGE_ORI: - - step: "test new exchange" - arm: - frame: exchanger - is_refer_planning_frame: true - rpy: [ 0., -1.5707963, 0. ] - drift_dimensions: [ true, true, true, true, true, true ] - tolerance_position: 0.01 - tolerance_orientation: 0.001 - spacial_shape: SPHERE - radius: 0.5 - point_resolution: 0.5 - max_planning_times: 1 - common: - <<: *NORMALLY - EXCHANGE_STRAIGHT: - - step: "test new exchange" - arm: - frame: exchanger - is_refer_planning_frame: true - xyz: [ -0.3, 0., 0. ] - rpy: [ 0., 3.1415926, 0. ] - drift_dimensions: [ true, true, true, true, true, true ] - tolerance_position: 0.01 - tolerance_orientation: 0.001 - spacial_shape: SPHERE - radius: 0.5 - point_resolution: 0.5 - max_planning_times: 1 - common: - <<: *NORMALLY - - GTESTL: - - step: "test" - gimbal_lifter: - target: *BUTTON_POS - GTESTM: - - step: "test" - gimbal_lifter: - target: *MID_POS - GTESTH: - - step: "test" - gimbal_lifter: - target: *TALLEST_POS - - ###################SMALL_ISLAND_FROM_TOP################ + ################### SMALL_ISLAND_FROM_SIDE ################ ONE_STONE_SMALL_ISLAND: - step: "GIMBAL_READY" gimbal: @@ -501,6 +373,9 @@ steps_list: - step: "ORE_LIFTER_DOWN" ore_lifter: target: *BIN_DOWN_POS + - step: "add stone" + stone_num: + change: "+1" ####FIRST_STONE### TWO_STONE_SMALL_ISLAND: @@ -579,6 +454,9 @@ steps_list: - step: "ORE_LIFTER_DOWN" ore_lifter: target: *BIN_DOWN_POS + - step: "add stone" + stone_num: + change: "+1" ###SECOND_STONE### TWO_STONE_SMALL_ISLAND00: - step: "SMALL_ARM_READY_TWO" @@ -644,8 +522,14 @@ steps_list: <<: *NORMALLY tolerance: <<: *NORMAL_TOLERANCE + - step: "add stone" + stone_num: + change: "+1" GET_DOWN_STONE_BIN: + - step: "GIMBAL_READY" + gimbal: + <<: *FRONT_POS - step: "ORE_ROTATOR_READY" ore_rotator: target: *EXCHANGE_POS @@ -683,8 +567,21 @@ steps_list: <<: *NORMALLY tolerance: <<: *NORMAL_TOLERANCE + - step: "exchange pos" + arm: + joints: [ *JOINT1_TWO_STONE_POSITION, *JOINT2_EXCHANGE_POSITION, *JOINT3_L_POSITION, *JOINT4_MID_POSITION, *JOINT5_MID_POSITION, *JOINT6_UP_POSITION, *JOINT7_MID_POSITION ] + common: + <<: *NORMALLY + tolerance: + <<: *NORMAL_TOLERANCE + - step: "remove stone" + stone_num: + change: "-1" GET_UP_STONE_BIN: + - step: "GIMBAL_READY" + gimbal: + <<: *FRONT_POS - step: "ORE_ROTATOR_READY" ore_rotator: target: *EXCHANGE_POS @@ -719,6 +616,16 @@ steps_list: <<: *NORMALLY tolerance: <<: *NORMAL_TOLERANCE + - step: "exchange pos" + arm: + joints: [ *JOINT1_TWO_STONE_POSITION, *JOINT2_EXCHANGE_POSITION, *JOINT3_L_POSITION, *JOINT4_MID_POSITION, *JOINT5_MID_POSITION, *JOINT6_UP_POSITION, *JOINT7_MID_POSITION ] + common: + <<: *NORMALLY + tolerance: + <<: *NORMAL_TOLERANCE + - step: "remove stone" + stone_num: + change: "-1" #################BIG_ISLAND############# MID_BIG_ISLAND: @@ -800,3 +707,92 @@ steps_list: - step: "ORE_LIFTER_DOWN" ore_lifter: target: *BIN_DOWN_POS + - step: "add stone" + stone_num: + change: "+1" + + + #################################################### TEST ########################################################## + + ########## EXCHANGE ############ + OLD_EXCHANGE_SPHERE: + - step: "close to a sphere" + arm: + frame: exchanger + xyz: [ -0.4, 0., -0.25 ] + rpy: [ 0., 0., 0. ] + rpy_rectify: [ 0.01,0.1,0.03 ] + drift_dimensions: [ true, true, true, true, true, true ] + tolerance_position: 0.1 + tolerance_orientation: 0.01 + spacial_shape: SPHERE + radius: 0.5 + point_resolution: 0.5 + max_planning_times: 1 + common: + <<: *QUICKLY + + EXCHANGE_DOWN: + - step: "test new exchange" + arm: + frame: exchanger + is_refer_planning_frame: true + xyz: [ -0.3, 0.2, -0.2 ] + rpy: [ 0., -1.5707963, 0. ] + drift_dimensions: [ true, true, true, true, true, true ] + tolerance_position: 0.01 + tolerance_orientation: 0.001 + spacial_shape: SPHERE + radius: 0.3 + point_resolution: 0.5 + max_planning_times: 1 + common: + <<: *NORMALLY + + TEST_ORI: + - step: "ori test" + arm: + frame: exchanger + rpy: [ 0., -1.5707963, 0. ] + tolerance_orientation: 0.001 + + TEST_ORI2: + - step: "ori test2" + arm: + frame: exchanger + rpy: [ 0., -1.5707963, 0. ] + xyz: [ -0.3, 0.2, -0.2 ] + tolerance_position: 0.01 + tolerance_orientation: 0.001 + + EXCHANGE_ORI: + - step: "test new exchange" + arm: + frame: exchanger + is_refer_planning_frame: true + rpy: [ 0., -1.5707963, 0. ] + drift_dimensions: [ true, true, true, true, true, true ] + tolerance_position: 0.01 + tolerance_orientation: 0.001 + spacial_shape: SPHERE + radius: 0.5 + point_resolution: 0.5 + max_planning_times: 1 + common: + <<: *NORMALLY + EXCHANGE_STRAIGHT: + - step: "test new exchange" + arm: + frame: exchanger + is_refer_planning_frame: true + xyz: [ -0.3, 0., 0. ] + rpy: [ 0., 3.1415926, 0. ] + drift_dimensions: [ true, true, true, true, true, true ] + tolerance_position: 0.01 + tolerance_orientation: 0.001 + spacial_shape: SPHERE + radius: 0.5 + point_resolution: 0.5 + max_planning_times: 1 + common: + <<: *NORMALLY From fb6bc3bafa200a4dfa46410c72dfbe019e8289d2 Mon Sep 17 00:00:00 2001 From: cc0h <7921166012@qq.com> Date: Mon, 8 Apr 2024 22:52:22 +0800 Subject: [PATCH 16/25] update steps_list.yaml. --- engineer_middleware/config/steps_list.yaml | 13 ++++++++----- 1 file changed, 8 insertions(+), 5 deletions(-) diff --git a/engineer_middleware/config/steps_list.yaml b/engineer_middleware/config/steps_list.yaml index 525c465..75478c9 100644 --- a/engineer_middleware/config/steps_list.yaml +++ b/engineer_middleware/config/steps_list.yaml @@ -166,10 +166,10 @@ common: gimbal: side_pos: &SIDE_POS - frame: gimbal_lifter + frame: pitch position: [ 0.001 , -3, 0. ] front_pos: &FRONT_POS - frame: gimbal_lifter + frame: pitch position: [ 2, 0.001, 0. ] gimbal_lifter: @@ -254,7 +254,7 @@ steps_list: ############################ HOME ########################## HOME_ZERO_STONE: - - step: "gimbal look side" + - step: "gimbal look front" gimbal: <<: *FRONT_POS - step: "close gripper" @@ -265,7 +265,7 @@ steps_list: <<: *HOME_1 variable: [ *JOINT1_ZERO_STONE_POSITION, 0, 0, 0, 0, 0 ] HOME_ONE_STONE: - - step: "gimbal look side" + - step: "gimbal look front" gimbal: <<: *FRONT_POS - step: "close gripper" @@ -276,7 +276,7 @@ steps_list: <<: *HOME_1 variable: [ *JOINT1_ONE_STONE_POSITION, 0, 0, 0, 0, 0 ] HOME_TWO_STONE: - - step: "gimbal look side" + - step: "gimbal look front" gimbal: <<: *FRONT_POS - step: "close gripper" @@ -376,6 +376,9 @@ steps_list: - step: "add stone" stone_num: change: "+1" + - step: "GIMBAL_READY" + gimbal: + <<: *SIDE_POS ####FIRST_STONE### TWO_STONE_SMALL_ISLAND: From 2382cd4d33b96391d5b0f72c9b079ff5cab72b71 Mon Sep 17 00:00:00 2001 From: cc0h <7921166012@qq.com> Date: Tue, 9 Apr 2024 23:54:21 +0800 Subject: [PATCH 17/25] Add support of extend arms in middleware. --- .../launch/load_controllers.launch | 4 +++ .../config/simulation_controllers.yaml | 8 ++--- engineer_middleware/config/steps_list.yaml | 29 +++++++++++++++++++ .../include/engineer_middleware/middleware.h | 3 +- .../include/engineer_middleware/step.h | 13 +++++++-- .../include/engineer_middleware/step_queue.h | 5 ++-- engineer_middleware/src/middleware.cpp | 6 ++-- 7 files changed, 57 insertions(+), 11 deletions(-) diff --git a/engineer_arm_config/launch/load_controllers.launch b/engineer_arm_config/launch/load_controllers.launch index 169d896..69275b2 100644 --- a/engineer_arm_config/launch/load_controllers.launch +++ b/engineer_arm_config/launch/load_controllers.launch @@ -32,6 +32,10 @@ controllers/gpio_controller controllers/gimbal_controller controllers/ore_bin_rotate_controller + controllers/extend_arm_front_controller + controllers/extend_arm_back_controller + controllers/extend_arm_front_calibration_controller + controllers/extend_arm_back_calibration_controller controllers/ore_bin_lifter_controller controllers/ore_bin_lifter_calibration_controller controllers/gimbal_lifter_controller diff --git a/engineer_middleware/config/simulation_controllers.yaml b/engineer_middleware/config/simulation_controllers.yaml index e873175..6271b5b 100644 --- a/engineer_middleware/config/simulation_controllers.yaml +++ b/engineer_middleware/config/simulation_controllers.yaml @@ -114,12 +114,12 @@ controllers: joint: gimbal_lifter_joint pid: { p: 20000., i: 0., d: 1200, i_max: 0.01, i_min: 0.01, antiwindup: true, publish_state: true } - extend_arm_a_controller: + extend_arm_front_controller: type: effort_controllers/JointPositionController - joint: extend_arm_a_joint + joint: extend_arm_front_joint pid: { p: 3., i: 0.1, d: 0.05, i_max: 0.01, i_min: 0.01, antiwindup: true, publish_state: true } - extend_arm_b_controller: + extend_arm_back_controller: type: effort_controllers/JointPositionController - joint: extend_arm_b_joint + joint: extend_arm_back_joint pid: { p: 3., i: 0.1, d: 0.05, i_max: 0.01, i_min: 0.01, antiwindup: true, publish_state: true } diff --git a/engineer_middleware/config/steps_list.yaml b/engineer_middleware/config/steps_list.yaml index 75478c9..de130d3 100644 --- a/engineer_middleware/config/steps_list.yaml +++ b/engineer_middleware/config/steps_list.yaml @@ -799,3 +799,32 @@ steps_list: max_planning_times: 1 common: <<: *NORMALLY + TEST11: + - step: "test" + extend_arm_front: + target: 1 + - step: "test" + extend_arm_back: + target: 1 + + TEST10: + - step: "test" + extend_arm_front: + target: 1 + - step: "test" + extend_arm_back: + target: 0 + TEST01: + - step: "test" + extend_arm_front: + target: 0 + - step: "test" + extend_arm_back: + target: 1 + TEST00: + - step: "test" + extend_arm_front: + target: 0 + - step: "test" + extend_arm_back: + target: 0 diff --git a/engineer_middleware/include/engineer_middleware/middleware.h b/engineer_middleware/include/engineer_middleware/middleware.h index 58bcb2d..cc9b809 100644 --- a/engineer_middleware/include/engineer_middleware/middleware.h +++ b/engineer_middleware/include/engineer_middleware/middleware.h @@ -82,7 +82,8 @@ class Middleware moveit::planning_interface::MoveGroupInterface arm_group_; ChassisInterface chassis_interface_; ros::Publisher hand_pub_, end_effector_pub_, gimbal_pub_, gpio_pub_, reversal_pub_, planning_result_pub_, - stone_num_pub_, point_cloud_pub_, ore_rotate_pub_, ore_lift_pub_, gimbal_lift_pub; + stone_num_pub_, point_cloud_pub_, ore_rotate_pub_, ore_lift_pub_, gimbal_lift_pub_, extend_arm_f_pub_, + extend_arm_b_pub_; std::unordered_map step_queues_; tf2_ros::Buffer tf_; tf2_ros::TransformListener tf_listener_; diff --git a/engineer_middleware/include/engineer_middleware/step.h b/engineer_middleware/include/engineer_middleware/step.h index e04ff2a..b94ea72 100644 --- a/engineer_middleware/include/engineer_middleware/step.h +++ b/engineer_middleware/include/engineer_middleware/step.h @@ -51,7 +51,7 @@ class Step ros::Publisher& hand_pub, ros::Publisher& end_effector_pub, ros::Publisher& gimbal_pub, ros::Publisher& gpio_pub, ros::Publisher& reversal_pub, ros::Publisher& stone_num_pub, ros::Publisher& planning_result_pub, ros::Publisher& point_cloud_pub, ros::Publisher& ore_rotate_pub, ros::Publisher& ore_lift_pub, - ros::Publisher& gimbal_lift_pub) + ros::Publisher& gimbal_lift_pub, ros::Publisher& extend_arm_f_pub, ros::Publisher& extend_arm_b_pub) : planning_result_pub_(planning_result_pub), point_cloud_pub_(point_cloud_pub), arm_group_(arm_group) { ROS_ASSERT(step.hasMember("step")); @@ -91,6 +91,10 @@ class Step ore_lift_motion_ = new JointPointMotion(step["ore_lifter"], ore_lift_pub); if (step.hasMember("gimbal_lifter")) gimbal_lift_motion_ = new JointPointMotion(step["gimbal_lifter"], gimbal_lift_pub); + if (step.hasMember("extend_arm_front")) + extend_arm_front_motion_ = new JointPointMotion(step["extend_arm_front"], extend_arm_f_pub); + if (step.hasMember("extend_arm_back")) + extend_arm_back_motion_ = new JointPointMotion(step["extend_arm_back"], extend_arm_b_pub); } bool move() { @@ -128,6 +132,10 @@ class Step success &= ore_rotate_motion_->move(); if (gimbal_lift_motion_) success &= gimbal_lift_motion_->move(); + if (extend_arm_back_motion_) + success &= extend_arm_back_motion_->move(); + if (extend_arm_front_motion_) + success &= extend_arm_front_motion_->move(); return success; } void stop() @@ -195,7 +203,8 @@ class Step MoveitMotionBase* arm_motion_{}; HandMotion* hand_motion_{}; JointPositionMotion* end_effector_motion_{}; - JointPointMotion *ore_rotate_motion_{}, *ore_lift_motion_{}, *gimbal_lift_motion_{}; + JointPointMotion *ore_rotate_motion_{}, *ore_lift_motion_{}, *gimbal_lift_motion_{}, *extend_arm_front_motion_{}, + *extend_arm_back_motion_{}; StoneNumMotion* stone_num_motion_{}; ChassisMotion* chassis_motion_{}; GimbalMotion* gimbal_motion_{}; diff --git a/engineer_middleware/include/engineer_middleware/step_queue.h b/engineer_middleware/include/engineer_middleware/step_queue.h index 2c6b63c..d2fa912 100644 --- a/engineer_middleware/include/engineer_middleware/step_queue.h +++ b/engineer_middleware/include/engineer_middleware/step_queue.h @@ -56,14 +56,15 @@ class StepQueue ros::Publisher& hand_pub, ros::Publisher& end_effector_pub, ros::Publisher& stone_num_pub, ros::Publisher& gimbal_pub, ros::Publisher& gpio_pub, ros::Publisher& reversal_pub, ros::Publisher& planning_result_pub, ros::Publisher& point_cloud_pub, ros::Publisher& ore_rotate_pub, - ros::Publisher& ore_lift_pub, ros::Publisher& gimbal_lift_pub) + ros::Publisher& ore_lift_pub, ros::Publisher& gimbal_lift_pub, ros::Publisher& extend_arm_f_pub, + ros::Publisher& extend_arm_b_pub) : chassis_interface_(chassis_interface) { ROS_ASSERT(steps.getType() == XmlRpc::XmlRpcValue::TypeArray); for (int i = 0; i < steps.size(); ++i) queue_.emplace_back(steps[i], scenes, tf, arm_group, chassis_interface, hand_pub, end_effector_pub, stone_num_pub, gimbal_pub, gpio_pub, reversal_pub, planning_result_pub, point_cloud_pub, ore_rotate_pub, - ore_lift_pub, gimbal_lift_pub); + ore_lift_pub, gimbal_lift_pub, extend_arm_f_pub, extend_arm_b_pub); } bool run(actionlib::SimpleActionServer& as) { diff --git a/engineer_middleware/src/middleware.cpp b/engineer_middleware/src/middleware.cpp index 701ade3..3ef4368 100644 --- a/engineer_middleware/src/middleware.cpp +++ b/engineer_middleware/src/middleware.cpp @@ -55,7 +55,9 @@ Middleware::Middleware(ros::NodeHandle& nh) , point_cloud_pub_(nh.advertise("/cloud", 100)) , ore_rotate_pub_(nh.advertise("/controllers/ore_bin_rotate_controller/command", 10)) , ore_lift_pub_(nh.advertise("/controllers/ore_bin_lifter_controller/command", 10)) - , gimbal_lift_pub(nh.advertise("/controllers/gimbal_lifter_controller/command", 10)) + , gimbal_lift_pub_(nh.advertise("/controllers/gimbal_lifter_controller/command", 10)) + , extend_arm_f_pub_(nh.advertise("/controllers/extend_arm_front_controller/command", 10)) + , extend_arm_b_pub_(nh.advertise("/controllers/extend_arm_back_controller/command", 10)) , tf_listener_(tf_) , is_middleware_control_(false) { @@ -73,7 +75,7 @@ Middleware::Middleware(ros::NodeHandle& nh) std::make_pair(it->first, StepQueue(it->second, scenes_list, tf_, arm_group_, chassis_interface_, hand_pub_, end_effector_pub_, gimbal_pub_, gpio_pub_, reversal_pub_, stone_num_pub_, planning_result_pub_, point_cloud_pub_, ore_rotate_pub_, ore_lift_pub_, - gimbal_lift_pub))); + gimbal_lift_pub_, extend_arm_f_pub_, extend_arm_b_pub_))); } } else From d0ca289e1d166e72f01370f1892c2a23252bfcb5 Mon Sep 17 00:00:00 2001 From: cc0h <7921166012@qq.com> Date: Wed, 10 Apr 2024 11:51:42 +0800 Subject: [PATCH 18/25] Add extend arm and gimbal lifter motion. --- engineer_middleware/config/steps_list.yaml | 56 ++++++++++++++++++++-- 1 file changed, 51 insertions(+), 5 deletions(-) diff --git a/engineer_middleware/config/steps_list.yaml b/engineer_middleware/config/steps_list.yaml index de130d3..325a608 100644 --- a/engineer_middleware/config/steps_list.yaml +++ b/engineer_middleware/config/steps_list.yaml @@ -166,15 +166,20 @@ common: gimbal: side_pos: &SIDE_POS - frame: pitch + frame: gimbal_lifter position: [ 0.001 , -3, 0. ] front_pos: &FRONT_POS - frame: pitch + frame: gimbal_lifter position: [ 2, 0.001, 0. ] + small_island_pos: &SMALL_ISLAND_POS + frame: gimbal_lifter + position: [ 1, 1, 0.] gimbal_lifter: button_pos: &BUTTON_POS - 0.01 + 0.05 + small_island: &LIFTER_SMALL_ISLAND + 0.06 mid_pos: &MID_POS 0.1 tallest_pos: &TALLEST_POS @@ -228,6 +233,11 @@ common: 0.1 bin_down_pos: &BIN_DOWN_POS 0.001 + extend_arm: + close: &EX_CLOSE + 0.0 + open: &EX_OPEN + 2.6 steps_list: EXCHANGE_POS: @@ -257,6 +267,15 @@ steps_list: - step: "gimbal look front" gimbal: <<: *FRONT_POS + - step: "gimbal lifter down" + gimbal_lifter: + target: *BUTTON_POS + - step: "close ex arm" + extend_arm_front: + target: *EX_CLOSE + - step: "close ex arm" + extend_arm_back: + target: *EX_CLOSE - step: "close gripper" gripper: <<: *CLOSE_GRIPPER @@ -268,6 +287,15 @@ steps_list: - step: "gimbal look front" gimbal: <<: *FRONT_POS + - step: "gimbal lifter down" + gimbal_lifter: + target: *MID_POS + - step: "close ex arm" + extend_arm_front: + target: *EX_CLOSE + - step: "close ex arm" + extend_arm_back: + target: *EX_CLOSE - step: "close gripper" gripper: <<: *CLOSE_GRIPPER @@ -279,6 +307,15 @@ steps_list: - step: "gimbal look front" gimbal: <<: *FRONT_POS + - step: "gimbal lifter down" + gimbal_lifter: + target: *MID_POS + - step: "close ex arm" + extend_arm_front: + target: *EX_CLOSE + - step: "close ex arm" + extend_arm_back: + target: *EX_CLOSE - step: "close gripper" gripper: <<: *CLOSE_GRIPPER @@ -300,7 +337,7 @@ steps_list: ONE_STONE_SMALL_ISLAND: - step: "GIMBAL_READY" gimbal: - <<: *SIDE_POS + <<: *SMALL_ISLAND_POS - step: "ORE_ROTATOR_READY" ore_rotator: target: *READY_POS @@ -384,7 +421,7 @@ steps_list: TWO_STONE_SMALL_ISLAND: - step: "GIMBAL_READY" gimbal: - <<: *SIDE_POS + <<: *SMALL_ISLAND_POS - step: "ORE_ROTATOR_READY" ore_rotator: target: *READY_POS @@ -632,6 +669,12 @@ steps_list: #################BIG_ISLAND############# MID_BIG_ISLAND: + - step: "open ex arm" + extend_arm_front: + target: *EX_OPEN + - step: "open ex arm" + extend_arm_back: + target: *EX_OPEN - step: "MID_ARM_READY" arm: joints: [ *JOINT1_BIG_READY, *JOINT2_BIG_READY, *JOINT3_BIG_READY, *JOINT4_R_POSITION, *JOINT5_BIG_READY, *JOINT6_BIG_READY, *JOINT7_BIG_POS ] @@ -700,6 +743,9 @@ steps_list: - step: "CLOSE_GRIPPER" gripper: <<: *CLOSE_GRIPPER + - step: "move arm out" + arm: + joints: ["KEEP", ] - step: "ARM_RESET" arm: joints: [ *JOINT1_BIG_READY, *JOINT2_BIG_READY, *JOINT3_GET_BIG_MID, *JOINT4_R_POSITION, *JOINT5_BIG_READY, *JOINT6_BIG_READY, *JOINT7_BIG_POS ] From c0c16fcc8174756a8f8b1fafbe85b04935afb6ac Mon Sep 17 00:00:00 2001 From: cc0h <7921166012@qq.com> Date: Thu, 11 Apr 2024 04:52:13 +0800 Subject: [PATCH 19/25] Fix steps list bug. --- engineer_middleware/config/steps_list.yaml | 169 ++++++++++++++------- 1 file changed, 112 insertions(+), 57 deletions(-) diff --git a/engineer_middleware/config/steps_list.yaml b/engineer_middleware/config/steps_list.yaml index 325a608..169b022 100644 --- a/engineer_middleware/config/steps_list.yaml +++ b/engineer_middleware/config/steps_list.yaml @@ -26,7 +26,7 @@ common: joint1: mechanical: down_position: &JOINT1_DOWN_POSITION - 0.015 + 0.02 small_ready: &JOINT1_SMALL_READY 0.15 up_pos: &JOINT1_UP_POS @@ -45,7 +45,7 @@ common: 0.155 home: zero_stone_position: &JOINT1_ZERO_STONE_POSITION - 0.08 + 0.01 one_stone_position: &JOINT1_ONE_STONE_POSITION 0.156 two_stone_position: &JOINT1_TWO_STONE_POSITION @@ -85,11 +85,13 @@ common: left_position: &JOINT3_L_POSITION 0.28 mid_position: &JOINT3_MID_POSITION - 0.0001 + 0.04 right_position: &JOINT3_R_POSITION -0.275 + home: &JOINT3_HOME_POS + 0.08 small_ready: &JOINT3_SMALL_READY - 0.098 + 0.208 small_ready_second: &JOINT3_SMALL_READY_SECOND 0.03 small_get_stone: &JOINT3_SMALL_GET_STONE @@ -122,7 +124,7 @@ common: joint5: mechanical: mid_position: &JOINT5_MID_POSITION - 0.001 + 0.011 left_90_position: &JOINT5_L90_POSITION -1.54 right_90_position: &JOINT5_R90_POSITION @@ -156,7 +158,7 @@ common: joint7: mechanical: mid_position: &JOINT7_MID_POSITION - 0.001 + 0.044 left_90_position: &JOINT7_L90_POSITION -1.59 right_90_position: &JOINT7_R90_POSITION @@ -173,15 +175,19 @@ common: position: [ 2, 0.001, 0. ] small_island_pos: &SMALL_ISLAND_POS frame: gimbal_lifter - position: [ 1, 1, 0.] + position: [ 1, -1, 0.] gimbal_lifter: button_pos: &BUTTON_POS - 0.05 + 0.07 small_island: &LIFTER_SMALL_ISLAND - 0.06 + 0.1 mid_pos: &MID_POS 0.1 + big_island: &LIFTER_BIG_ISLAND + 0.185 + two_stone_pos: &TWO_STONE_POS + 0.163 tallest_pos: &TALLEST_POS 0.28 @@ -195,7 +201,7 @@ common: arm: home: &HOME_1 - joints: [ "VARIABLE", *JOINT2_BACK_POSITION, *JOINT3_MID_POSITION, *JOINT4_R_POSITION, *JOINT5_MID_POSITION, *JOINT6_UP_POSITION, *JOINT7_MID_POSITION ] + joints: [ "VARIABLE", *JOINT2_BACK_POSITION, *JOINT3_HOME_POS, *JOINT4_R_POSITION, *JOINT5_MID_POSITION, *JOINT6_UP_POSITION, *JOINT7_MID_POSITION ] common: <<: *NORMALLY tolerance: @@ -203,9 +209,9 @@ common: chassis_move: - chassis_backward_30: &CHASSIS_BACKWARD_25 + chassis_left_315: &CHASSIS_LEFT_15 frame: base_link - position: [ -0.35,0. ] + position: [ 0., 15. ] yaw: 0.0 chassis_tolerance_position: 0.1 chassis_tolerance_angular: 0.3 @@ -246,11 +252,14 @@ steps_list: <<: *FRONT_POS - step: "move arm" arm: - joints: [*JOINT1_TWO_STONE_POSITION, *JOINT2_EXCHANGE_POSITION, *JOINT3_L_POSITION, *JOINT4_MID_POSITION, *JOINT5_MID_POSITION, *JOINT6_UP_POSITION, *JOINT7_MID_POSITION] + joints: [*JOINT1_DOWN_POSITION, *JOINT2_EXCHANGE_POSITION, *JOINT3_MID_POSITION, *JOINT4_MID_POSITION, *JOINT5_MID_POSITION, *JOINT6_UP_POSITION, *JOINT7_MID_POSITION] common: <<: *NORMALLY tolerance: <<: *NORMAL_TOLERANCE + - step: "gimbal lifter tallest" + gimbal_lifter: + target: *TALLEST_POS GIMBAL_FRONT: - step: "gimbal look front" @@ -279,6 +288,12 @@ steps_list: - step: "close gripper" gripper: <<: *CLOSE_GRIPPER + - step: "ORE_ROTATOR_READY" + ore_rotator: + target: *READY_POS + - step: "ORE_LIFTER_UP" + ore_lifter: + target: *BIN_DOWN_POS - step: "home with no stone" arm: <<: *HOME_1 @@ -299,10 +314,16 @@ steps_list: - step: "close gripper" gripper: <<: *CLOSE_GRIPPER + - step: "ORE_ROTATOR_READY" + ore_rotator: + target: *READY_POS + - step: "ORE_LIFTER_UP" + ore_lifter: + target: *BIN_DOWN_POS - step: "home with one stone" arm: <<: *HOME_1 - variable: [ *JOINT1_ONE_STONE_POSITION, 0, 0, 0, 0, 0 ] + variable: [ *JOINT1_ZERO_STONE_POSITION, 0, 0, 0, 0, 0 ] HOME_TWO_STONE: - step: "gimbal look front" gimbal: @@ -319,10 +340,16 @@ steps_list: - step: "close gripper" gripper: <<: *CLOSE_GRIPPER + - step: "ORE_ROTATOR_READY" + ore_rotator: + target: *READY_POS + - step: "ORE_LIFTER_UP" + ore_lifter: + target: *BIN_DOWN_POS - step: "home with two stone" arm: <<: *HOME_1 - variable: [ *JOINT1_TWO_STONE_POSITION, 0, 0, 0, 0, 0 ] + variable: [ *JOINT1_ZERO_STONE_POSITION, 0, 0, 0, 0, 0 ] OPEN_GRIPPER: - step: "open gripper" @@ -346,15 +373,23 @@ steps_list: target: *BIN_MID_POS - step: "SMALL_ARM_READY" arm: - joints: [ *JOINT1_SMALL_READY, *JOINT2_BACK_POSITION, *JOINT3_SMALL_READY, *JOINT4_R_POSITION, *JOINT5_MID_POSITION, *JOINT6_MID_POSITION, *JOINT7_MID_POSITION ] + joints: [ *JOINT1_DOWN_POSITION, *JOINT2_BACK_POSITION, *JOINT3_SMALL_READY, *JOINT4_R_POSITION, *JOINT5_MID_POSITION, *JOINT6_MID_POSITION, *JOINT7_MID_POSITION ] common: <<: *NORMALLY tolerance: <<: *NORMAL_TOLERANCE + - step: "OPEN_GRIPPER" gripper: <<: *OPEN_GRIPPER ONE_STONE_SMALL_ISLAND0: + - step: "SMALL_ARM_READY" + arm: + joints: [ *JOINT1_SMALL_READY, *JOINT2_BACK_POSITION, *JOINT3_SMALL_READY, *JOINT4_R_POSITION, *JOINT5_MID_POSITION, *JOINT6_MID_POSITION, *JOINT7_MID_POSITION ] + common: + <<: *NORMALLY + tolerance: + <<: *NORMAL_TOLERANCE - step: "SMALL_GET_STONE" arm: joints: [ *JOINT1_SMALL_READY, *JOINT2_BACK_POSITION, *JOINT3_SMALL_GET_STONE, *JOINT4_R_POSITION, *JOINT5_MID_POSITION, *JOINT6_MID_POSITION, *JOINT7_MID_POSITION ] @@ -407,6 +442,13 @@ steps_list: <<: *NORMALLY tolerance: <<: *NORMAL_TOLERANCE + - step: "home" + arm: + joints: [ *JOINT1_ZERO_STONE_POSITION, *JOINT2_BACK_POSITION, *JOINT3_HOME_POS, *JOINT4_R_POSITION, *JOINT5_MID_POSITION, *JOINT6_UP_POSITION, *JOINT7_MID_POSITION ] + common: + <<: *NORMALLY + tolerance: + <<: *NORMAL_TOLERANCE - step: "ORE_LIFTER_DOWN" ore_lifter: target: *BIN_DOWN_POS @@ -415,7 +457,7 @@ steps_list: change: "+1" - step: "GIMBAL_READY" gimbal: - <<: *SIDE_POS + <<: *FRONT_POS ####FIRST_STONE### TWO_STONE_SMALL_ISLAND: @@ -430,7 +472,7 @@ steps_list: target: *BIN_MID_POS - step: "SMALL_ARM_READY" arm: - joints: [ *JOINT1_SMALL_READY, *JOINT2_BACK_POSITION, *JOINT3_SMALL_READY, *JOINT4_R_POSITION, *JOINT5_MID_POSITION, *JOINT6_MID_POSITION, *JOINT7_MID_POSITION ] + joints: [ *JOINT1_DOWN_POSITION, *JOINT2_BACK_POSITION, *JOINT3_SMALL_READY, *JOINT4_R_POSITION, *JOINT5_MID_POSITION, *JOINT6_MID_POSITION, *JOINT7_MID_POSITION ] common: <<: *NORMALLY tolerance: @@ -439,6 +481,13 @@ steps_list: gripper: <<: *OPEN_GRIPPER TWO_STONE_SMALL_ISLAND0: + - step: "SMALL_ARM_READY" + arm: + joints: [ *JOINT1_SMALL_READY, *JOINT2_BACK_POSITION, *JOINT3_SMALL_READY, *JOINT4_R_POSITION, *JOINT5_MID_POSITION, *JOINT6_MID_POSITION, *JOINT7_MID_POSITION ] + common: + <<: *NORMALLY + tolerance: + <<: *NORMAL_TOLERANCE - step: "SMALL_GET_STONE" arm: joints: [ *JOINT1_SMALL_READY, *JOINT2_BACK_POSITION, *JOINT3_SMALL_GET_STONE, *JOINT4_R_POSITION, *JOINT5_MID_POSITION, *JOINT6_MID_POSITION, *JOINT7_MID_POSITION ] @@ -562,6 +611,13 @@ steps_list: <<: *NORMALLY tolerance: <<: *NORMAL_TOLERANCE + - step: "home" + arm: + joints: [ *JOINT1_ZERO_STONE_POSITION, *JOINT2_BACK_POSITION, *JOINT3_HOME_POS, *JOINT4_R_POSITION, *JOINT5_MID_POSITION, *JOINT6_UP_POSITION, *JOINT7_MID_POSITION ] + common: + <<: *NORMALLY + tolerance: + <<: *NORMAL_TOLERANCE - step: "add stone" stone_num: change: "+1" @@ -609,11 +665,14 @@ steps_list: <<: *NORMAL_TOLERANCE - step: "exchange pos" arm: - joints: [ *JOINT1_TWO_STONE_POSITION, *JOINT2_EXCHANGE_POSITION, *JOINT3_L_POSITION, *JOINT4_MID_POSITION, *JOINT5_MID_POSITION, *JOINT6_UP_POSITION, *JOINT7_MID_POSITION ] + joints: [ *JOINT1_DOWN_POSITION, *JOINT2_EXCHANGE_POSITION, *JOINT3_MID_POSITION, *JOINT4_MID_POSITION, *JOINT5_MID_POSITION, *JOINT6_UP_POSITION, *JOINT7_MID_POSITION ] common: <<: *NORMALLY tolerance: <<: *NORMAL_TOLERANCE + - step: "gimbal lifter tallest" + gimbal_lifter: + target: *TALLEST_POS - step: "remove stone" stone_num: change: "-1" @@ -658,17 +717,23 @@ steps_list: <<: *NORMAL_TOLERANCE - step: "exchange pos" arm: - joints: [ *JOINT1_TWO_STONE_POSITION, *JOINT2_EXCHANGE_POSITION, *JOINT3_L_POSITION, *JOINT4_MID_POSITION, *JOINT5_MID_POSITION, *JOINT6_UP_POSITION, *JOINT7_MID_POSITION ] + joints: [ *JOINT1_DOWN_POSITION, *JOINT2_EXCHANGE_POSITION, *JOINT3_MID_POSITION, *JOINT4_MID_POSITION, *JOINT5_MID_POSITION, *JOINT6_UP_POSITION, *JOINT7_MID_POSITION ] common: <<: *NORMALLY tolerance: <<: *NORMAL_TOLERANCE + - step: "gimbal lifter tallest" + gimbal_lifter: + target: *TALLEST_POS - step: "remove stone" stone_num: change: "-1" #################BIG_ISLAND############# MID_BIG_ISLAND: + - step: "gimbal ready" + gimbal: + <<: *SMALL_ISLAND_POS - step: "open ex arm" extend_arm_front: target: *EX_OPEN @@ -688,9 +753,13 @@ steps_list: - step: "ORE_LIFTER_READY" ore_lifter: target: *BIN_MID_POS + - step: "gimbal lifter" + gimbal_lifter: + target: *LIFTER_BIG_ISLAND - step: "OPEN_GRIPPER" gripper: <<: *OPEN_GRIPPER + MID_BIG_ISLAND0: - step: "GET_MID_STONE" arm: joints: [ *JOINT1_BIG_READY, *JOINT2_BIG_READY, *JOINT3_GET_BIG_MID, *JOINT4_R_POSITION, *JOINT5_BIG_READY, *JOINT6_BIG_READY, *JOINT7_BIG_POS ] @@ -712,6 +781,9 @@ steps_list: <<: *SLOWLY tolerance: <<: *NORMAL_TOLERANCE + - step: "chassis move left" + chassis: + <<: *CHASSIS_LEFT_15 - step: "JOINT1_UP" arm: joints: [ *JOINT1_BIG_UP_POS, "KEEP", "KEEP", "KEEP", "KEEP", "KEEP", "KEEP" ] @@ -745,21 +817,33 @@ steps_list: <<: *CLOSE_GRIPPER - step: "move arm out" arm: - joints: ["KEEP", ] - - step: "ARM_RESET" + joints: [ *JOINT1_DOWN_POSITION, *JOINT2_BACK_POSITION, "KEEP", *JOINT4_R_POSITION, *JOINT5_MID_POSITION, *JOINT6_UP_POSITION, *JOINT7_MID_POSITION ] + common: + <<: *NORMALLY + tolerance: + <<: *NORMAL_TOLERANCE + - step: "home" arm: - joints: [ *JOINT1_BIG_READY, *JOINT2_BIG_READY, *JOINT3_GET_BIG_MID, *JOINT4_R_POSITION, *JOINT5_BIG_READY, *JOINT6_BIG_READY, *JOINT7_BIG_POS ] - common: - <<: *SLOWLY - tolerance: - <<: *NORMAL_TOLERANCE + joints: [ *JOINT1_ZERO_STONE_POSITION, *JOINT2_BACK_POSITION, *JOINT3_HOME_POS, *JOINT4_R_POSITION, *JOINT5_MID_POSITION, *JOINT6_UP_POSITION, *JOINT7_MID_POSITION ] + common: + <<: *NORMALLY + tolerance: + <<: *NORMAL_TOLERANCE - step: "ORE_LIFTER_DOWN" ore_lifter: target: *BIN_DOWN_POS - step: "add stone" stone_num: change: "+1" - + - step: "open ex arm" + extend_arm_front: + target: *EX_CLOSE + - step: "open ex arm" + extend_arm_back: + target: *EX_CLOSE + - step: "gimbal front" + gimbal: + <<: *FRONT_POS #################################################### TEST ########################################################## @@ -845,32 +929,3 @@ steps_list: max_planning_times: 1 common: <<: *NORMALLY - TEST11: - - step: "test" - extend_arm_front: - target: 1 - - step: "test" - extend_arm_back: - target: 1 - - TEST10: - - step: "test" - extend_arm_front: - target: 1 - - step: "test" - extend_arm_back: - target: 0 - TEST01: - - step: "test" - extend_arm_front: - target: 0 - - step: "test" - extend_arm_back: - target: 1 - TEST00: - - step: "test" - extend_arm_front: - target: 0 - - step: "test" - extend_arm_back: - target: 0 From fedc09ff2e4b543ee76ea6fc9580583b3752c79e Mon Sep 17 00:00:00 2001 From: cc0h <7921166012@qq.com> Date: Sun, 14 Apr 2024 15:13:39 +0800 Subject: [PATCH 20/25] Replace KDL solver with TRAC-IK solver. --- engineer_arm_config/config/kinematics.yaml | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/engineer_arm_config/config/kinematics.yaml b/engineer_arm_config/config/kinematics.yaml index a4b51af..b298b37 100644 --- a/engineer_arm_config/config/kinematics.yaml +++ b/engineer_arm_config/config/kinematics.yaml @@ -1,4 +1,3 @@ engineer_arm: - kinematics_solver: kdl_kinematics_plugin/KDLKinematicsPlugin - kinematics_solver_search_resolution: 0.005 + kinematics_solver: trac_ik_kinematics_plugin/TRAC_IKKinematicsPlugin kinematics_solver_timeout: 0.005 From 1e78179e1996ba1eec86d8d9c03f96928248b9ed Mon Sep 17 00:00:00 2001 From: cc0h <7921166012@qq.com> Date: Sun, 14 Apr 2024 15:13:59 +0800 Subject: [PATCH 21/25] Record steps list. --- engineer_middleware/config/steps_list.yaml | 167 ++++++++++++++++++--- 1 file changed, 145 insertions(+), 22 deletions(-) diff --git a/engineer_middleware/config/steps_list.yaml b/engineer_middleware/config/steps_list.yaml index 169b022..d485ff8 100644 --- a/engineer_middleware/config/steps_list.yaml +++ b/engineer_middleware/config/steps_list.yaml @@ -28,17 +28,17 @@ common: down_position: &JOINT1_DOWN_POSITION 0.02 small_ready: &JOINT1_SMALL_READY - 0.15 + 0.16 up_pos: &JOINT1_UP_POS 0.312 small_ready_store: &JOINT1_SMALL_READY_STORE - 0.162 + 0.17 bin_get_stone: &JOINT1_BIN_GET_STONE 0.142 big_ready: &JOINT1_BIG_READY 0.006 big_adjust_mid: &JOINT1_BIG_ADJUST_MID - 0.071 + 0.08 big_up_pos: &JOINT1_BIG_UP_POS 0.320 big_ready_store: &JOINT1_BIG_READY_STORE @@ -78,7 +78,7 @@ common: big_arm_up: &JOINT2_BIG_ARM_UP 0.142 big_ready_store: &JOINT2_BIG_READY_STORE - 0.030 + 0.020 joint3: mechanical: @@ -95,21 +95,25 @@ common: small_ready_second: &JOINT3_SMALL_READY_SECOND 0.03 small_get_stone: &JOINT3_SMALL_GET_STONE - 0.034 + 0.047 + small_get_stone2: &JOINT3_SMALL_GET_STONE2 + 0.056 small_get_second: &JOINT3_SMALL_GET_SECOND -0.018 small_store_stone: &JOINT3_SMALL_STORE_STONE - 0.30 + 0.291 bin_get_stone: &JOINT3_BIN_GET_STONE 0.258 big_ready: &JOINT3_BIG_READY 0.071 get_big_mid: &JOINT3_GET_BIG_MID -0.281 + get_big_side: &JOINT3_GET_BIG_SIDE + -0.199 big_pull_out: &JOINT3_BIG_PULL_OUT 0.188 big_arm_up: &JOINT3_BIG_ARM_UP - 0.289 + 0.291 joint4: mechanical: @@ -162,7 +166,7 @@ common: left_90_position: &JOINT7_L90_POSITION -1.59 right_90_position: &JOINT7_R90_POSITION - 1.57 + 1.62 big_pos: &JOINT7_BIG_POS -0.013 @@ -183,13 +187,13 @@ common: small_island: &LIFTER_SMALL_ISLAND 0.1 mid_pos: &MID_POS - 0.1 + 0.14 big_island: &LIFTER_BIG_ISLAND - 0.185 + 0.260 two_stone_pos: &TWO_STONE_POS 0.163 tallest_pos: &TALLEST_POS - 0.28 + 0.394 gripper: open_gripper: &OPEN_GRIPPER @@ -397,16 +401,16 @@ steps_list: <<: *NORMALLY tolerance: <<: *NORMAL_TOLERANCE - - step: "JOINT1_UP" + - step: "JOINT1_UP AND BACK" arm: - joints: [ *JOINT1_UP_POS, "KEEP", "KEEP", "KEEP", "KEEP", "KEEP", "KEEP"] + joints: [ *JOINT1_UP_POS, "KEEP", *JOINT3_SMALL_GET_STONE2, "KEEP", "KEEP", "KEEP", "KEEP"] common: <<: *NORMALLY tolerance: <<: *NORMAL_TOLERANCE - step: "SMALL_ARM_UP_STONE" arm: - joints: [ *JOINT1_UP_POS, *JOINT2_SMALL_ARM_UP, *JOINT3_SMALL_GET_STONE, *JOINT4_R_POSITION, *JOINT5_MID_POSITION, *JOINT6_UP_POSITION, *JOINT7_MID_POSITION ] + joints: [ *JOINT1_UP_POS, *JOINT2_SMALL_ARM_UP, *JOINT3_SMALL_GET_STONE2, *JOINT4_R_POSITION, *JOINT5_MID_POSITION, *JOINT6_UP_POSITION, *JOINT7_MID_POSITION ] common: <<: *SLOWLY tolerance: @@ -554,7 +558,7 @@ steps_list: common: <<: *NORMALLY tolerance: - <<: *SMALL_TOLERANCE + <<: *NORMAL_TOLERANCE - step: "OPEN_GRIPPER" gripper: <<: *OPEN_GRIPPER @@ -781,9 +785,9 @@ steps_list: <<: *SLOWLY tolerance: <<: *NORMAL_TOLERANCE - - step: "chassis move left" - chassis: - <<: *CHASSIS_LEFT_15 +# - step: "chassis move left" +# chassis: +# <<: *CHASSIS_LEFT_15 - step: "JOINT1_UP" arm: joints: [ *JOINT1_BIG_UP_POS, "KEEP", "KEEP", "KEEP", "KEEP", "KEEP", "KEEP" ] @@ -835,12 +839,116 @@ steps_list: - step: "add stone" stone_num: change: "+1" + - step: "gimbal front" + gimbal: + <<: *FRONT_POS + + + SIDE_BIG_ISLAND: + - step: "gimbal ready" + gimbal: + <<: *SMALL_ISLAND_POS - step: "open ex arm" extend_arm_front: - target: *EX_CLOSE - - step: "open ex arm" - extend_arm_back: - target: *EX_CLOSE + target: *EX_OPEN +# - step: "open ex arm" +# extend_arm_back: +# target: *EX_OPEN + - step: "MID_ARM_READY" + arm: + joints: [ *JOINT1_BIG_READY, *JOINT2_BIG_READY, *JOINT3_BIG_READY, *JOINT4_R_POSITION, *JOINT5_BIG_READY, *JOINT6_BIG_READY, *JOINT7_BIG_POS ] + common: + <<: *SLOWLY + tolerance: + <<: *NORMAL_TOLERANCE + - step: "ORE_ROTATOR_READY" + ore_rotator: + target: *READY_POS + - step: "ORE_LIFTER_READY" + ore_lifter: + target: *BIN_MID_POS + - step: "gimbal lifter" + gimbal_lifter: + target: *LIFTER_BIG_ISLAND + - step: "OPEN_GRIPPER" + gripper: + <<: *OPEN_GRIPPER + SIDE_BIG_ISLAND0: + - step: "GET_MID_STONE" + arm: + joints: [ *JOINT1_BIG_READY, *JOINT2_BIG_READY, *JOINT3_GET_BIG_SIDE, *JOINT4_R_POSITION, *JOINT5_BIG_READY, *JOINT6_BIG_READY, *JOINT7_BIG_POS ] + common: + <<: *SLOWLY + tolerance: + <<: *NORMAL_TOLERANCE + - step: "ADJUST_STONE" + arm: + joints: [ *JOINT1_BIG_ADJUST_MID, *JOINT2_BIG_READY, *JOINT3_GET_BIG_SIDE, *JOINT4_R_POSITION, *JOINT5_BIG_READY, *JOINT6_BIG_ADJUST_MID, *JOINT7_BIG_POS ] + common: + <<: *SLOWLY + tolerance: + <<: *NORMAL_TOLERANCE + - step: "PULL_OUT_STONE" + arm: + joints: [ "KEEP", "KEEP", *JOINT3_BIG_PULL_OUT, "KEEP", "KEEP", "KEEP", "KEEP" ] + common: + <<: *SLOWLY + tolerance: + <<: *NORMAL_TOLERANCE + # - step: "chassis move left" + # chassis: + # <<: *CHASSIS_LEFT_15 + - step: "JOINT1_UP" + arm: + joints: [ *JOINT1_BIG_UP_POS, "KEEP", "KEEP", "KEEP", "KEEP", "KEEP", "KEEP" ] + common: + <<: *SLOWLY + tolerance: + <<: *NORMAL_TOLERANCE + - step: "ARM_UP_STONE" + arm: + joints: [ *JOINT1_BIG_UP_POS, *JOINT2_BIG_ARM_UP, *JOINT3_BIG_ARM_UP, *JOINT4_R_POSITION, *JOINT5_BIG_READY, *JOINT6_BIG_ARM_UP, *JOINT7_BIG_POS ] + common: + <<: *SLOWLY + tolerance: + <<: *NORMAL_TOLERANCE + - step: "READY_TO_STORE" + arm: + joints: [ *JOINT1_BIG_READY_STORE, *JOINT2_BIG_READY_STORE, *JOINT3_BIG_ARM_UP, *JOINT4_R_POSITION, *JOINT5_BIG_READY, *JOINT6_BIG_ARM_UP, *JOINT7_BIG_POS ] + common: + <<: *SLOWLY + tolerance: + <<: *NORMAL_TOLERANCE + - step: "STORE_STONE" + arm: + joints: [ *JOINT1_BIG_READY_STORE, *JOINT2_BIG_READY_STORE, *JOINT3_BIG_ARM_UP, *JOINT4_R_POSITION, *JOINT5_BIG_STORE, *JOINT6_BIG_ARM_UP, *JOINT7_BIG_POS ] + common: + <<: *SLOWLY + tolerance: + <<: *NORMAL_TOLERANCE + - step: "CLOSE_GRIPPER" + gripper: + <<: *CLOSE_GRIPPER + - step: "move arm out" + arm: + joints: [ *JOINT1_DOWN_POSITION, *JOINT2_BACK_POSITION, "KEEP", *JOINT4_R_POSITION, *JOINT5_MID_POSITION, *JOINT6_UP_POSITION, *JOINT7_MID_POSITION ] + common: + <<: *NORMALLY + tolerance: + <<: *NORMAL_TOLERANCE + - step: "home" + arm: + joints: [ *JOINT1_ZERO_STONE_POSITION, *JOINT2_BACK_POSITION, *JOINT3_HOME_POS, *JOINT4_R_POSITION, *JOINT5_MID_POSITION, *JOINT6_UP_POSITION, *JOINT7_MID_POSITION ] + common: + <<: *NORMALLY + tolerance: + <<: *NORMAL_TOLERANCE + - step: "ORE_LIFTER_DOWN" + ore_lifter: + target: *BIN_DOWN_POS + - step: "add stone" + stone_num: + change: "+1" - step: "gimbal front" gimbal: <<: *FRONT_POS @@ -929,3 +1037,18 @@ steps_list: max_planning_times: 1 common: <<: *NORMALLY + + GIMBAL_DOWN: + - step: "gimbal lowest" + gimbal_lifter: + target: *BUTTON_POS + + GIMBAL_MID: + - step: "gimbal mid" + gimbal_lifter: + target: *LIFTER_BIG_ISLAND + + GIMBAL_TALL: + - step: "gimbal tall" + gimbal_lifter: + target: *TALLEST_POS From c81a6efe13f929715e9b8f20d2780d41433a931b Mon Sep 17 00:00:00 2001 From: cc0h <7921166012@qq.com> Date: Sun, 14 Apr 2024 15:51:59 +0800 Subject: [PATCH 22/25] Disable collision check between joint3 and base_link. --- engineer_arm_config/config/engineer.srdf | 1 + 1 file changed, 1 insertion(+) diff --git a/engineer_arm_config/config/engineer.srdf b/engineer_arm_config/config/engineer.srdf index 604c9d1..c49a334 100644 --- a/engineer_arm_config/config/engineer.srdf +++ b/engineer_arm_config/config/engineer.srdf @@ -34,6 +34,7 @@ + From 5bbc02ece3fd22f7e3667ed001ceac16366ed952 Mon Sep 17 00:00:00 2001 From: 2194555 <3631676002@qq.com> Date: Sun, 14 Apr 2024 18:57:26 +0800 Subject: [PATCH 23/25] Merge extend_arm in step_list and add has_stone logic. --- engineer_middleware/config/engineer.yaml | 6 +- engineer_middleware/config/steps_list.yaml | 613 +++++++++++++++--- .../include/engineer_middleware/motion.h | 38 ++ .../include/engineer_middleware/step.h | 16 +- 4 files changed, 585 insertions(+), 88 deletions(-) diff --git a/engineer_middleware/config/engineer.yaml b/engineer_middleware/config/engineer.yaml index c63f929..73c761e 100644 --- a/engineer_middleware/config/engineer.yaml +++ b/engineer_middleware/config/engineer.yaml @@ -1,9 +1,9 @@ chassis: x: - pid: { p: 0.0, i: 0.0, d: 0.0, i_clamp_max: 0, i_clamp_min: 0, antiwindup: true, publish_state: true } + pid: { p: 4.5, i: 0.0, d: 0.2, i_clamp_max: 0, i_clamp_min: 0, antiwindup: true, publish_state: true } y: - pid: { p: 0.0, i: 0.0, d: 0.0, i_clamp_max: 0, i_clamp_min: 0, antiwindup: true, publish_state: true } + pid: { p: 4.5, i: 0.0, d: 0.2, i_clamp_max: 0, i_clamp_min: 0, antiwindup: true, publish_state: true } yaw: - pid: { p: 3., i: 0.0, d: 0.1, i_clamp_max: 0, i_clamp_min: 0, antiwindup: true, publish_state: true } + pid: { p: 5., i: 0.0, d: 0.05, i_clamp_max: 0, i_clamp_min: 0, antiwindup: true, publish_state: true } yaw_start_threshold: 0.05 max_vel: 0.1 diff --git a/engineer_middleware/config/steps_list.yaml b/engineer_middleware/config/steps_list.yaml index 325a608..ef72681 100644 --- a/engineer_middleware/config/steps_list.yaml +++ b/engineer_middleware/config/steps_list.yaml @@ -26,7 +26,7 @@ common: joint1: mechanical: down_position: &JOINT1_DOWN_POSITION - 0.015 + 0.02 small_ready: &JOINT1_SMALL_READY 0.15 up_pos: &JOINT1_UP_POS @@ -45,7 +45,7 @@ common: 0.155 home: zero_stone_position: &JOINT1_ZERO_STONE_POSITION - 0.08 + 0.01 one_stone_position: &JOINT1_ONE_STONE_POSITION 0.156 two_stone_position: &JOINT1_TWO_STONE_POSITION @@ -78,18 +78,20 @@ common: big_arm_up: &JOINT2_BIG_ARM_UP 0.142 big_ready_store: &JOINT2_BIG_READY_STORE - 0.030 + 0.020 joint3: mechanical: left_position: &JOINT3_L_POSITION 0.28 mid_position: &JOINT3_MID_POSITION - 0.0001 + 0.04 right_position: &JOINT3_R_POSITION -0.275 + home: &JOINT3_HOME_POS + 0.08 small_ready: &JOINT3_SMALL_READY - 0.098 + 0.208 small_ready_second: &JOINT3_SMALL_READY_SECOND 0.03 small_get_stone: &JOINT3_SMALL_GET_STONE @@ -104,10 +106,12 @@ common: 0.071 get_big_mid: &JOINT3_GET_BIG_MID -0.281 + get_big_side: &JOINT3_GET_BIG_SIDE + -0.199 big_pull_out: &JOINT3_BIG_PULL_OUT 0.188 big_arm_up: &JOINT3_BIG_ARM_UP - 0.289 + 0.291 joint4: mechanical: @@ -122,7 +126,7 @@ common: joint5: mechanical: mid_position: &JOINT5_MID_POSITION - 0.001 + 0.011 left_90_position: &JOINT5_L90_POSITION -1.54 right_90_position: &JOINT5_R90_POSITION @@ -156,7 +160,7 @@ common: joint7: mechanical: mid_position: &JOINT7_MID_POSITION - 0.001 + 0.044 left_90_position: &JOINT7_L90_POSITION -1.59 right_90_position: &JOINT7_R90_POSITION @@ -173,17 +177,21 @@ common: position: [ 2, 0.001, 0. ] small_island_pos: &SMALL_ISLAND_POS frame: gimbal_lifter - position: [ 1, 1, 0.] + position: [ 1, -1, 0.] gimbal_lifter: button_pos: &BUTTON_POS - 0.05 + 0.07 small_island: &LIFTER_SMALL_ISLAND - 0.06 - mid_pos: &MID_POS 0.1 + mid_pos: &MID_POS + 0.14 + big_island: &LIFTER_BIG_ISLAND + 0.260 + two_stone_pos: &TWO_STONE_POS + 0.163 tallest_pos: &TALLEST_POS - 0.28 + 0.394 gripper: open_gripper: &OPEN_GRIPPER @@ -195,7 +203,7 @@ common: arm: home: &HOME_1 - joints: [ "VARIABLE", *JOINT2_BACK_POSITION, *JOINT3_MID_POSITION, *JOINT4_R_POSITION, *JOINT5_MID_POSITION, *JOINT6_UP_POSITION, *JOINT7_MID_POSITION ] + joints: [ "VARIABLE", *JOINT2_BACK_POSITION, *JOINT3_HOME_POS, *JOINT4_R_POSITION, *JOINT5_MID_POSITION, *JOINT6_UP_POSITION, *JOINT7_MID_POSITION ] common: <<: *NORMALLY tolerance: @@ -203,9 +211,9 @@ common: chassis_move: - chassis_backward_30: &CHASSIS_BACKWARD_25 + chassis_left_315: &CHASSIS_LEFT_15 frame: base_link - position: [ -0.35,0. ] + position: [ 0., 15. ] yaw: 0.0 chassis_tolerance_position: 0.1 chassis_tolerance_angular: 0.3 @@ -246,11 +254,14 @@ steps_list: <<: *FRONT_POS - step: "move arm" arm: - joints: [*JOINT1_TWO_STONE_POSITION, *JOINT2_EXCHANGE_POSITION, *JOINT3_L_POSITION, *JOINT4_MID_POSITION, *JOINT5_MID_POSITION, *JOINT6_UP_POSITION, *JOINT7_MID_POSITION] + joints: [*JOINT1_DOWN_POSITION, *JOINT2_EXCHANGE_POSITION, *JOINT3_MID_POSITION, *JOINT4_MID_POSITION, *JOINT5_MID_POSITION, *JOINT6_UP_POSITION, *JOINT7_MID_POSITION] common: <<: *NORMALLY tolerance: <<: *NORMAL_TOLERANCE + - step: "gimbal lifter tallest" + gimbal_lifter: + target: *TALLEST_POS GIMBAL_FRONT: - step: "gimbal look front" @@ -272,13 +283,17 @@ steps_list: target: *BUTTON_POS - step: "close ex arm" extend_arm_front: - target: *EX_CLOSE - - step: "close ex arm" - extend_arm_back: - target: *EX_CLOSE + front: *EX_CLOSE + back: *EX_CLOSE - step: "close gripper" gripper: <<: *CLOSE_GRIPPER + - step: "ORE_ROTATOR_READY" + ore_rotator: + target: *READY_POS + - step: "ORE_LIFTER_UP" + ore_lifter: + target: *BIN_DOWN_POS - step: "home with no stone" arm: <<: *HOME_1 @@ -292,17 +307,21 @@ steps_list: target: *MID_POS - step: "close ex arm" extend_arm_front: - target: *EX_CLOSE - - step: "close ex arm" - extend_arm_back: - target: *EX_CLOSE + front: *EX_CLOSE + back: *EX_CLOSE - step: "close gripper" gripper: <<: *CLOSE_GRIPPER + - step: "ORE_ROTATOR_READY" + ore_rotator: + target: *READY_POS + - step: "ORE_LIFTER_UP" + ore_lifter: + target: *BIN_DOWN_POS - step: "home with one stone" arm: <<: *HOME_1 - variable: [ *JOINT1_ONE_STONE_POSITION, 0, 0, 0, 0, 0 ] + variable: [ *JOINT1_ZERO_STONE_POSITION, 0, 0, 0, 0, 0 ] HOME_TWO_STONE: - step: "gimbal look front" gimbal: @@ -312,17 +331,21 @@ steps_list: target: *MID_POS - step: "close ex arm" extend_arm_front: - target: *EX_CLOSE - - step: "close ex arm" - extend_arm_back: - target: *EX_CLOSE + front: *EX_CLOSE + back: *EX_CLOSE - step: "close gripper" gripper: <<: *CLOSE_GRIPPER + - step: "ORE_ROTATOR_READY" + ore_rotator: + target: *READY_POS + - step: "ORE_LIFTER_UP" + ore_lifter: + target: *BIN_DOWN_POS - step: "home with two stone" arm: <<: *HOME_1 - variable: [ *JOINT1_TWO_STONE_POSITION, 0, 0, 0, 0, 0 ] + variable: [ *JOINT1_ZERO_STONE_POSITION, 0, 0, 0, 0, 0 ] OPEN_GRIPPER: - step: "open gripper" @@ -334,7 +357,8 @@ steps_list: <<: *CLOSE_GRIPPER ################### SMALL_ISLAND_FROM_SIDE ################ - ONE_STONE_SMALL_ISLAND: + ########ONE_STONE_ONCE######### + HAS0_ONE_STONE_SMALL_ISLAND: - step: "GIMBAL_READY" gimbal: <<: *SMALL_ISLAND_POS @@ -346,15 +370,23 @@ steps_list: target: *BIN_MID_POS - step: "SMALL_ARM_READY" arm: - joints: [ *JOINT1_SMALL_READY, *JOINT2_BACK_POSITION, *JOINT3_SMALL_READY, *JOINT4_R_POSITION, *JOINT5_MID_POSITION, *JOINT6_MID_POSITION, *JOINT7_MID_POSITION ] + joints: [ *JOINT1_DOWN_POSITION, *JOINT2_BACK_POSITION, *JOINT3_SMALL_READY, *JOINT4_R_POSITION, *JOINT5_MID_POSITION, *JOINT6_MID_POSITION, *JOINT7_MID_POSITION ] common: <<: *NORMALLY tolerance: <<: *NORMAL_TOLERANCE + - step: "OPEN_GRIPPER" gripper: <<: *OPEN_GRIPPER - ONE_STONE_SMALL_ISLAND0: + HAS0_ONE_STONE_SMALL_ISLAND0: + - step: "SMALL_ARM_READY" + arm: + joints: [ *JOINT1_SMALL_READY, *JOINT2_BACK_POSITION, *JOINT3_SMALL_READY, *JOINT4_R_POSITION, *JOINT5_MID_POSITION, *JOINT6_MID_POSITION, *JOINT7_MID_POSITION ] + common: + <<: *NORMALLY + tolerance: + <<: *NORMAL_TOLERANCE - step: "SMALL_GET_STONE" arm: joints: [ *JOINT1_SMALL_READY, *JOINT2_BACK_POSITION, *JOINT3_SMALL_GET_STONE, *JOINT4_R_POSITION, *JOINT5_MID_POSITION, *JOINT6_MID_POSITION, *JOINT7_MID_POSITION ] @@ -407,6 +439,13 @@ steps_list: <<: *NORMALLY tolerance: <<: *NORMAL_TOLERANCE + - step: "home" + arm: + joints: [ *JOINT1_ZERO_STONE_POSITION, *JOINT2_BACK_POSITION, *JOINT3_HOME_POS, *JOINT4_R_POSITION, *JOINT5_MID_POSITION, *JOINT6_UP_POSITION, *JOINT7_MID_POSITION ] + common: + <<: *NORMALLY + tolerance: + <<: *NORMAL_TOLERANCE - step: "ORE_LIFTER_DOWN" ore_lifter: target: *BIN_DOWN_POS @@ -415,8 +454,103 @@ steps_list: change: "+1" - step: "GIMBAL_READY" gimbal: - <<: *SIDE_POS + <<: *FRONT_POS + + HAS1_ONE_STONE_SMALL_ISLAND: + - step: "GIMBAL_READY" + gimbal: + <<: *SMALL_ISLAND_POS + - step: "ORE_ROTATOR_READY" + ore_rotator: + target: *READY_POS + - step: "ORE_LIFTER_READY" + ore_lifter: + target: *BIN_DOWN_POS + - step: "SMALL_ARM_READY" + arm: + joints: [ *JOINT1_DOWN_POSITION, *JOINT2_BACK_POSITION, *JOINT3_SMALL_READY, *JOINT4_R_POSITION, *JOINT5_MID_POSITION, *JOINT6_MID_POSITION, *JOINT7_MID_POSITION ] + common: + <<: *NORMALLY + tolerance: + <<: *NORMAL_TOLERANCE + - step: "OPEN_GRIPPER" + gripper: + <<: *OPEN_GRIPPER + ONE_STONE_SMALL_ISLAND0: + - step: "SMALL_ARM_READY" + arm: + joints: [ *JOINT1_SMALL_READY, *JOINT2_BACK_POSITION, *JOINT3_SMALL_READY, *JOINT4_R_POSITION, *JOINT5_MID_POSITION, *JOINT6_MID_POSITION, *JOINT7_MID_POSITION ] + common: + <<: *NORMALLY + tolerance: + <<: *NORMAL_TOLERANCE + - step: "SMALL_GET_STONE" + arm: + joints: [ *JOINT1_SMALL_READY, *JOINT2_BACK_POSITION, *JOINT3_SMALL_GET_STONE, *JOINT4_R_POSITION, *JOINT5_MID_POSITION, *JOINT6_MID_POSITION, *JOINT7_MID_POSITION ] + common: + <<: *NORMALLY + tolerance: + <<: *NORMAL_TOLERANCE + - step: "JOINT1_UP" + arm: + joints: [ *JOINT1_UP_POS, "KEEP", "KEEP", "KEEP", "KEEP", "KEEP", "KEEP"] + common: + <<: *NORMALLY + tolerance: + <<: *NORMAL_TOLERANCE + - step: "SMALL_ARM_UP_STONE" + arm: + joints: [ *JOINT1_UP_POS, *JOINT2_SMALL_ARM_UP, *JOINT3_SMALL_GET_STONE, *JOINT4_R_POSITION, *JOINT5_MID_POSITION, *JOINT6_UP_POSITION, *JOINT7_MID_POSITION ] + common: + <<: *SLOWLY + tolerance: + <<: *NORMAL_TOLERANCE + - step: "READY_TO_STORE_STONE" + arm: + joints: [ *JOINT1_SMALL_READY_STORE, *JOINT2_SMALL_ARM_UP, *JOINT3_SMALL_STORE_STONE, *JOINT4_R_POSITION, *JOINT5_MID_POSITION, *JOINT6_UP_POSITION, *JOINT7_MID_POSITION ] + common: + <<: *NORMALLY + tolerance: + <<: *SMALL_TOLERANCE + - step: "STORE_STONE" + arm: + joints: [ *JOINT1_SMALL_READY_STORE, *JOINT2_SMALL_STORE_STONE, *JOINT3_SMALL_STORE_STONE, *JOINT4_R_POSITION, *JOINT5_MID_POSITION, *JOINT6_UP_POSITION, *JOINT7_MID_POSITION ] + common: + <<: *SLOWLY + tolerance: + <<: *SMALL_TOLERANCE + - step: "JOINT5_TURN" + arm: + joints: [ *JOINT1_SMALL_READY_STORE, *JOINT2_SMALL_STORE_STONE, *JOINT3_SMALL_STORE_STONE, *JOINT4_R_POSITION, *JOINT5_R90_POSITION, *JOINT6_UP_POSITION, *JOINT7_MID_POSITION ] + common: + <<: *SLOWLY + tolerance: + <<: *SMALL_TOLERANCE + - step: "CLOSE_GRIPPER" + gripper: + <<: *CLOSE_GRIPPER + - step: "ARM_RESET" + arm: + joints: [ *JOINT1_SMALL_READY_STORE, *JOINT2_SMALL_STORE_STONE, *JOINT3_L_POSITION, *JOINT4_R_POSITION, *JOINT5_MID_POSITION, *JOINT6_MID_POSITION, *JOINT7_MID_POSITION ] + common: + <<: *NORMALLY + tolerance: + <<: *NORMAL_TOLERANCE + - step: "home" + arm: + joints: [ *JOINT1_ZERO_STONE_POSITION, *JOINT2_BACK_POSITION, *JOINT3_HOME_POS, *JOINT4_R_POSITION, *JOINT5_MID_POSITION, *JOINT6_UP_POSITION, *JOINT7_MID_POSITION ] + common: + <<: *NORMALLY + tolerance: + <<: *NORMAL_TOLERANCE + - step: "add stone" + stone_num: + change: "+1" + - step: "GIMBAL_READY" + gimbal: + <<: *FRONT_POS + ##########TWO_STONE_ONCE########### ####FIRST_STONE### TWO_STONE_SMALL_ISLAND: - step: "GIMBAL_READY" @@ -430,7 +564,7 @@ steps_list: target: *BIN_MID_POS - step: "SMALL_ARM_READY" arm: - joints: [ *JOINT1_SMALL_READY, *JOINT2_BACK_POSITION, *JOINT3_SMALL_READY, *JOINT4_R_POSITION, *JOINT5_MID_POSITION, *JOINT6_MID_POSITION, *JOINT7_MID_POSITION ] + joints: [ *JOINT1_DOWN_POSITION, *JOINT2_BACK_POSITION, *JOINT3_SMALL_READY, *JOINT4_R_POSITION, *JOINT5_MID_POSITION, *JOINT6_MID_POSITION, *JOINT7_MID_POSITION ] common: <<: *NORMALLY tolerance: @@ -439,6 +573,13 @@ steps_list: gripper: <<: *OPEN_GRIPPER TWO_STONE_SMALL_ISLAND0: + - step: "SMALL_ARM_READY" + arm: + joints: [ *JOINT1_SMALL_READY, *JOINT2_BACK_POSITION, *JOINT3_SMALL_READY, *JOINT4_R_POSITION, *JOINT5_MID_POSITION, *JOINT6_MID_POSITION, *JOINT7_MID_POSITION ] + common: + <<: *NORMALLY + tolerance: + <<: *NORMAL_TOLERANCE - step: "SMALL_GET_STONE" arm: joints: [ *JOINT1_SMALL_READY, *JOINT2_BACK_POSITION, *JOINT3_SMALL_GET_STONE, *JOINT4_R_POSITION, *JOINT5_MID_POSITION, *JOINT6_MID_POSITION, *JOINT7_MID_POSITION ] @@ -505,7 +646,7 @@ steps_list: common: <<: *NORMALLY tolerance: - <<: *SMALL_TOLERANCE + <<: *NORMAL_TOLERANCE - step: "OPEN_GRIPPER" gripper: <<: *OPEN_GRIPPER @@ -562,6 +703,13 @@ steps_list: <<: *NORMALLY tolerance: <<: *NORMAL_TOLERANCE + - step: "home" + arm: + joints: [ *JOINT1_ZERO_STONE_POSITION, *JOINT2_BACK_POSITION, *JOINT3_HOME_POS, *JOINT4_R_POSITION, *JOINT5_MID_POSITION, *JOINT6_UP_POSITION, *JOINT7_MID_POSITION ] + common: + <<: *NORMALLY + tolerance: + <<: *NORMAL_TOLERANCE - step: "add stone" stone_num: change: "+1" @@ -609,11 +757,14 @@ steps_list: <<: *NORMAL_TOLERANCE - step: "exchange pos" arm: - joints: [ *JOINT1_TWO_STONE_POSITION, *JOINT2_EXCHANGE_POSITION, *JOINT3_L_POSITION, *JOINT4_MID_POSITION, *JOINT5_MID_POSITION, *JOINT6_UP_POSITION, *JOINT7_MID_POSITION ] + joints: [ *JOINT1_DOWN_POSITION, *JOINT2_EXCHANGE_POSITION, *JOINT3_MID_POSITION, *JOINT4_MID_POSITION, *JOINT5_MID_POSITION, *JOINT6_UP_POSITION, *JOINT7_MID_POSITION ] common: <<: *NORMALLY tolerance: <<: *NORMAL_TOLERANCE + - step: "gimbal lifter tallest" + gimbal_lifter: + target: *TALLEST_POS - step: "remove stone" stone_num: change: "-1" @@ -658,23 +809,28 @@ steps_list: <<: *NORMAL_TOLERANCE - step: "exchange pos" arm: - joints: [ *JOINT1_TWO_STONE_POSITION, *JOINT2_EXCHANGE_POSITION, *JOINT3_L_POSITION, *JOINT4_MID_POSITION, *JOINT5_MID_POSITION, *JOINT6_UP_POSITION, *JOINT7_MID_POSITION ] + joints: [ *JOINT1_DOWN_POSITION, *JOINT2_EXCHANGE_POSITION, *JOINT3_MID_POSITION, *JOINT4_MID_POSITION, *JOINT5_MID_POSITION, *JOINT6_UP_POSITION, *JOINT7_MID_POSITION ] common: <<: *NORMALLY tolerance: <<: *NORMAL_TOLERANCE + - step: "gimbal lifter tallest" + gimbal_lifter: + target: *TALLEST_POS - step: "remove stone" stone_num: change: "-1" #################BIG_ISLAND############# - MID_BIG_ISLAND: - - step: "open ex arm" - extend_arm_front: - target: *EX_OPEN + ######MID_STONE####### + HAS0_MID_BIG_ISLAND: + - step: "gimbal ready" + gimbal: + <<: *SMALL_ISLAND_POS - step: "open ex arm" - extend_arm_back: - target: *EX_OPEN + extend_arm: + front: *EX_OPEN + back: *EX_OPEN - step: "MID_ARM_READY" arm: joints: [ *JOINT1_BIG_READY, *JOINT2_BIG_READY, *JOINT3_BIG_READY, *JOINT4_R_POSITION, *JOINT5_BIG_READY, *JOINT6_BIG_READY, *JOINT7_BIG_POS ] @@ -688,9 +844,13 @@ steps_list: - step: "ORE_LIFTER_READY" ore_lifter: target: *BIN_MID_POS + - step: "gimbal lifter" + gimbal_lifter: + target: *LIFTER_BIG_ISLAND - step: "OPEN_GRIPPER" gripper: <<: *OPEN_GRIPPER + HAS0_MID_BIG_ISLAND0: - step: "GET_MID_STONE" arm: joints: [ *JOINT1_BIG_READY, *JOINT2_BIG_READY, *JOINT3_GET_BIG_MID, *JOINT4_R_POSITION, *JOINT5_BIG_READY, *JOINT6_BIG_READY, *JOINT7_BIG_POS ] @@ -712,6 +872,9 @@ steps_list: <<: *SLOWLY tolerance: <<: *NORMAL_TOLERANCE +# - step: "chassis move left" +# chassis: +# <<: *CHASSIS_LEFT_15 - step: "JOINT1_UP" arm: joints: [ *JOINT1_BIG_UP_POS, "KEEP", "KEEP", "KEEP", "KEEP", "KEEP", "KEEP" ] @@ -745,21 +908,340 @@ steps_list: <<: *CLOSE_GRIPPER - step: "move arm out" arm: - joints: ["KEEP", ] - - step: "ARM_RESET" + joints: [ *JOINT1_DOWN_POSITION, *JOINT2_BACK_POSITION, "KEEP", *JOINT4_R_POSITION, *JOINT5_MID_POSITION, *JOINT6_UP_POSITION, *JOINT7_MID_POSITION ] + common: + <<: *NORMALLY + tolerance: + <<: *NORMAL_TOLERANCE + - step: "home" + arm: + joints: [ *JOINT1_ZERO_STONE_POSITION, *JOINT2_BACK_POSITION, *JOINT3_HOME_POS, *JOINT4_R_POSITION, *JOINT5_MID_POSITION, *JOINT6_UP_POSITION, *JOINT7_MID_POSITION ] + common: + <<: *NORMALLY + tolerance: + <<: *NORMAL_TOLERANCE + - step: "ORE_LIFTER_DOWN" + ore_lifter: + target: *BIN_DOWN_POS + - step: "add stone" + stone_num: + change: "+1" + - step: "gimbal front" + gimbal: + <<: *FRONT_POS + + HAS1_MID_BIG_ISLAND: + - step: "gimbal ready" + gimbal: + <<: *SMALL_ISLAND_POS + - step: "open ex arm" + extend_arm: + front: *EX_OPEN + back: *EX_OPEN + - step: "MID_ARM_READY" + arm: + joints: [ *JOINT1_BIG_READY, *JOINT2_BIG_READY, *JOINT3_BIG_READY, *JOINT4_R_POSITION, *JOINT5_BIG_READY, *JOINT6_BIG_READY, *JOINT7_BIG_POS ] + common: + <<: *SLOWLY + tolerance: + <<: *NORMAL_TOLERANCE + - step: "ORE_ROTATOR_READY" + ore_rotator: + target: *READY_POS + - step: "ORE_LIFTER_READY" + ore_lifter: + target: *BIN_DOWN_POS + - step: "gimbal lifter" + gimbal_lifter: + target: *LIFTER_BIG_ISLAND + - step: "OPEN_GRIPPER" + gripper: + <<: *OPEN_GRIPPER + HAS1_MID_BIG_ISLAND0: + - step: "GET_MID_STONE" arm: joints: [ *JOINT1_BIG_READY, *JOINT2_BIG_READY, *JOINT3_GET_BIG_MID, *JOINT4_R_POSITION, *JOINT5_BIG_READY, *JOINT6_BIG_READY, *JOINT7_BIG_POS ] - common: - <<: *SLOWLY - tolerance: - <<: *NORMAL_TOLERANCE + common: + <<: *SLOWLY + tolerance: + <<: *NORMAL_TOLERANCE + - step: "ADJUST_STONE" + arm: + joints: [ *JOINT1_BIG_ADJUST_MID, *JOINT2_BIG_READY, *JOINT3_GET_BIG_MID, *JOINT4_R_POSITION, *JOINT5_BIG_READY, *JOINT6_BIG_ADJUST_MID, *JOINT7_BIG_POS ] + common: + <<: *SLOWLY + tolerance: + <<: *NORMAL_TOLERANCE + - step: "PULL_OUT_STONE" + arm: + joints: [ "KEEP", "KEEP", *JOINT3_BIG_PULL_OUT, "KEEP", "KEEP", "KEEP", "KEEP" ] + common: + <<: *SLOWLY + tolerance: + <<: *NORMAL_TOLERANCE + # - step: "chassis move left" + # chassis: + # <<: *CHASSIS_LEFT_15 + - step: "JOINT1_UP" + arm: + joints: [ *JOINT1_BIG_UP_POS, "KEEP", "KEEP", "KEEP", "KEEP", "KEEP", "KEEP" ] + common: + <<: *SLOWLY + tolerance: + <<: *NORMAL_TOLERANCE + - step: "ARM_UP_STONE" + arm: + joints: [ *JOINT1_BIG_UP_POS, *JOINT2_BIG_ARM_UP, *JOINT3_BIG_ARM_UP, *JOINT4_R_POSITION, *JOINT5_BIG_READY, *JOINT6_BIG_ARM_UP, *JOINT7_BIG_POS ] + common: + <<: *SLOWLY + tolerance: + <<: *NORMAL_TOLERANCE + - step: "READY_TO_STORE" + arm: + joints: [ *JOINT1_BIG_READY_STORE, *JOINT2_BIG_READY_STORE, *JOINT3_BIG_ARM_UP, *JOINT4_R_POSITION, *JOINT5_BIG_READY, *JOINT6_BIG_ARM_UP, *JOINT7_BIG_POS ] + common: + <<: *SLOWLY + tolerance: + <<: *NORMAL_TOLERANCE + - step: "STORE_STONE" + arm: + joints: [ *JOINT1_BIG_READY_STORE, *JOINT2_BIG_READY_STORE, *JOINT3_BIG_ARM_UP, *JOINT4_R_POSITION, *JOINT5_BIG_STORE, *JOINT6_BIG_ARM_UP, *JOINT7_BIG_POS ] + common: + <<: *SLOWLY + tolerance: + <<: *NORMAL_TOLERANCE + - step: "CLOSE_GRIPPER" + gripper: + <<: *CLOSE_GRIPPER + - step: "move arm out" + arm: + joints: [ *JOINT1_DOWN_POSITION, *JOINT2_BACK_POSITION, "KEEP", *JOINT4_R_POSITION, *JOINT5_MID_POSITION, *JOINT6_UP_POSITION, *JOINT7_MID_POSITION ] + common: + <<: *NORMALLY + tolerance: + <<: *NORMAL_TOLERANCE + - step: "home" + arm: + joints: [ *JOINT1_ZERO_STONE_POSITION, *JOINT2_BACK_POSITION, *JOINT3_HOME_POS, *JOINT4_R_POSITION, *JOINT5_MID_POSITION, *JOINT6_UP_POSITION, *JOINT7_MID_POSITION ] + common: + <<: *NORMALLY + tolerance: + <<: *NORMAL_TOLERANCE + - step: "add stone" + stone_num: + change: "+1" + - step: "gimbal front" + gimbal: + <<: *FRONT_POS + ########SIDE_STONE######### + HAS0_SIDE_BIG_ISLAND: + - step: "gimbal ready" + gimbal: + <<: *SMALL_ISLAND_POS + - step: "open ex arm" + extend_arm: + front: *EX_OPEN + - step: "MID_ARM_READY" + arm: + joints: [ *JOINT1_BIG_READY, *JOINT2_BIG_READY, *JOINT3_BIG_READY, *JOINT4_R_POSITION, *JOINT5_BIG_READY, *JOINT6_BIG_READY, *JOINT7_BIG_POS ] + common: + <<: *SLOWLY + tolerance: + <<: *NORMAL_TOLERANCE + - step: "ORE_ROTATOR_READY" + ore_rotator: + target: *READY_POS + - step: "ORE_LIFTER_READY" + ore_lifter: + target: *BIN_MID_POS + - step: "gimbal lifter" + gimbal_lifter: + target: *LIFTER_BIG_ISLAND + - step: "OPEN_GRIPPER" + gripper: + <<: *OPEN_GRIPPER + HAS0_SIDE_BIG_ISLAND0: + - step: "GET_MID_STONE" + arm: + joints: [ *JOINT1_BIG_READY, *JOINT2_BIG_READY, *JOINT3_GET_BIG_SIDE, *JOINT4_R_POSITION, *JOINT5_BIG_READY, *JOINT6_BIG_READY, *JOINT7_BIG_POS ] + common: + <<: *SLOWLY + tolerance: + <<: *NORMAL_TOLERANCE + - step: "ADJUST_STONE" + arm: + joints: [ *JOINT1_BIG_ADJUST_MID, *JOINT2_BIG_READY, *JOINT3_GET_BIG_SIDE, *JOINT4_R_POSITION, *JOINT5_BIG_READY, *JOINT6_BIG_ADJUST_MID, *JOINT7_BIG_POS ] + common: + <<: *SLOWLY + tolerance: + <<: *NORMAL_TOLERANCE + - step: "PULL_OUT_STONE" + arm: + joints: [ "KEEP", "KEEP", *JOINT3_BIG_PULL_OUT, "KEEP", "KEEP", "KEEP", "KEEP" ] + common: + <<: *SLOWLY + tolerance: + <<: *NORMAL_TOLERANCE + # - step: "chassis move left" + # chassis: + # <<: *CHASSIS_LEFT_15 + - step: "JOINT1_UP" + arm: + joints: [ *JOINT1_BIG_UP_POS, "KEEP", "KEEP", "KEEP", "KEEP", "KEEP", "KEEP" ] + common: + <<: *SLOWLY + tolerance: + <<: *NORMAL_TOLERANCE + - step: "ARM_UP_STONE" + arm: + joints: [ *JOINT1_BIG_UP_POS, *JOINT2_BIG_ARM_UP, *JOINT3_BIG_ARM_UP, *JOINT4_R_POSITION, *JOINT5_BIG_READY, *JOINT6_BIG_ARM_UP, *JOINT7_BIG_POS ] + common: + <<: *SLOWLY + tolerance: + <<: *NORMAL_TOLERANCE + - step: "READY_TO_STORE" + arm: + joints: [ *JOINT1_BIG_READY_STORE, *JOINT2_BIG_READY_STORE, *JOINT3_BIG_ARM_UP, *JOINT4_R_POSITION, *JOINT5_BIG_READY, *JOINT6_BIG_ARM_UP, *JOINT7_BIG_POS ] + common: + <<: *SLOWLY + tolerance: + <<: *NORMAL_TOLERANCE + - step: "STORE_STONE" + arm: + joints: [ *JOINT1_BIG_READY_STORE, *JOINT2_BIG_READY_STORE, *JOINT3_BIG_ARM_UP, *JOINT4_R_POSITION, *JOINT5_BIG_STORE, *JOINT6_BIG_ARM_UP, *JOINT7_BIG_POS ] + common: + <<: *SLOWLY + tolerance: + <<: *NORMAL_TOLERANCE + - step: "CLOSE_GRIPPER" + gripper: + <<: *CLOSE_GRIPPER + - step: "move arm out" + arm: + joints: [ *JOINT1_DOWN_POSITION, *JOINT2_BACK_POSITION, "KEEP", *JOINT4_R_POSITION, *JOINT5_MID_POSITION, *JOINT6_UP_POSITION, *JOINT7_MID_POSITION ] + common: + <<: *NORMALLY + tolerance: + <<: *NORMAL_TOLERANCE + - step: "home" + arm: + joints: [ *JOINT1_ZERO_STONE_POSITION, *JOINT2_BACK_POSITION, *JOINT3_HOME_POS, *JOINT4_R_POSITION, *JOINT5_MID_POSITION, *JOINT6_UP_POSITION, *JOINT7_MID_POSITION ] + common: + <<: *NORMALLY + tolerance: + <<: *NORMAL_TOLERANCE - step: "ORE_LIFTER_DOWN" ore_lifter: target: *BIN_DOWN_POS - step: "add stone" stone_num: change: "+1" + - step: "gimbal front" + gimbal: + <<: *FRONT_POS + HAS1_SIDE_BIG_ISLAND: + - step: "gimbal ready" + gimbal: + <<: *SMALL_ISLAND_POS + - step: "open ex arm" + extend_arm: + front: *EX_OPEN + - step: "MID_ARM_READY" + arm: + joints: [ *JOINT1_BIG_READY, *JOINT2_BIG_READY, *JOINT3_BIG_READY, *JOINT4_R_POSITION, *JOINT5_BIG_READY, *JOINT6_BIG_READY, *JOINT7_BIG_POS ] + common: + <<: *SLOWLY + tolerance: + <<: *NORMAL_TOLERANCE + - step: "ORE_ROTATOR_READY" + ore_rotator: + target: *READY_POS + - step: "ORE_LIFTER_READY" + ore_lifter: + target: *BIN_DOWN_POS + - step: "gimbal lifter" + gimbal_lifter: + target: *LIFTER_BIG_ISLAND + - step: "OPEN_GRIPPER" + gripper: + <<: *OPEN_GRIPPER + HAS1_SIDE_BIG_ISLAND0: + - step: "GET_MID_STONE" + arm: + joints: [ *JOINT1_BIG_READY, *JOINT2_BIG_READY, *JOINT3_GET_BIG_SIDE, *JOINT4_R_POSITION, *JOINT5_BIG_READY, *JOINT6_BIG_READY, *JOINT7_BIG_POS ] + common: + <<: *SLOWLY + tolerance: + <<: *NORMAL_TOLERANCE + - step: "ADJUST_STONE" + arm: + joints: [ *JOINT1_BIG_ADJUST_MID, *JOINT2_BIG_READY, *JOINT3_GET_BIG_SIDE, *JOINT4_R_POSITION, *JOINT5_BIG_READY, *JOINT6_BIG_ADJUST_MID, *JOINT7_BIG_POS ] + common: + <<: *SLOWLY + tolerance: + <<: *NORMAL_TOLERANCE + - step: "PULL_OUT_STONE" + arm: + joints: [ "KEEP", "KEEP", *JOINT3_BIG_PULL_OUT, "KEEP", "KEEP", "KEEP", "KEEP" ] + common: + <<: *SLOWLY + tolerance: + <<: *NORMAL_TOLERANCE + # - step: "chassis move left" + # chassis: + # <<: *CHASSIS_LEFT_15 + - step: "JOINT1_UP" + arm: + joints: [ *JOINT1_BIG_UP_POS, "KEEP", "KEEP", "KEEP", "KEEP", "KEEP", "KEEP" ] + common: + <<: *SLOWLY + tolerance: + <<: *NORMAL_TOLERANCE + - step: "ARM_UP_STONE" + arm: + joints: [ *JOINT1_BIG_UP_POS, *JOINT2_BIG_ARM_UP, *JOINT3_BIG_ARM_UP, *JOINT4_R_POSITION, *JOINT5_BIG_READY, *JOINT6_BIG_ARM_UP, *JOINT7_BIG_POS ] + common: + <<: *SLOWLY + tolerance: + <<: *NORMAL_TOLERANCE + - step: "READY_TO_STORE" + arm: + joints: [ *JOINT1_BIG_READY_STORE, *JOINT2_BIG_READY_STORE, *JOINT3_BIG_ARM_UP, *JOINT4_R_POSITION, *JOINT5_BIG_READY, *JOINT6_BIG_ARM_UP, *JOINT7_BIG_POS ] + common: + <<: *SLOWLY + tolerance: + <<: *NORMAL_TOLERANCE + - step: "STORE_STONE" + arm: + joints: [ *JOINT1_BIG_READY_STORE, *JOINT2_BIG_READY_STORE, *JOINT3_BIG_ARM_UP, *JOINT4_R_POSITION, *JOINT5_BIG_STORE, *JOINT6_BIG_ARM_UP, *JOINT7_BIG_POS ] + common: + <<: *SLOWLY + tolerance: + <<: *NORMAL_TOLERANCE + - step: "CLOSE_GRIPPER" + gripper: + <<: *CLOSE_GRIPPER + - step: "move arm out" + arm: + joints: [ *JOINT1_DOWN_POSITION, *JOINT2_BACK_POSITION, "KEEP", *JOINT4_R_POSITION, *JOINT5_MID_POSITION, *JOINT6_UP_POSITION, *JOINT7_MID_POSITION ] + common: + <<: *NORMALLY + tolerance: + <<: *NORMAL_TOLERANCE + - step: "home" + arm: + joints: [ *JOINT1_ZERO_STONE_POSITION, *JOINT2_BACK_POSITION, *JOINT3_HOME_POS, *JOINT4_R_POSITION, *JOINT5_MID_POSITION, *JOINT6_UP_POSITION, *JOINT7_MID_POSITION ] + common: + <<: *NORMALLY + tolerance: + <<: *NORMAL_TOLERANCE + - step: "add stone" + stone_num: + change: "+1" + - step: "gimbal front" + gimbal: + <<: *FRONT_POS #################################################### TEST ########################################################## @@ -845,32 +1327,5 @@ steps_list: max_planning_times: 1 common: <<: *NORMALLY - TEST11: - - step: "test" - extend_arm_front: - target: 1 - - step: "test" - extend_arm_back: - target: 1 - TEST10: - - step: "test" - extend_arm_front: - target: 1 - - step: "test" - extend_arm_back: - target: 0 - TEST01: - - step: "test" - extend_arm_front: - target: 0 - - step: "test" - extend_arm_back: - target: 1 - TEST00: - - step: "test" - extend_arm_front: - target: 0 - - step: "test" - extend_arm_back: - target: 0 + diff --git a/engineer_middleware/include/engineer_middleware/motion.h b/engineer_middleware/include/engineer_middleware/motion.h index c893f49..4a1870b 100644 --- a/engineer_middleware/include/engineer_middleware/motion.h +++ b/engineer_middleware/include/engineer_middleware/motion.h @@ -720,4 +720,42 @@ class JointPointMotion : public PublishMotion double target_; }; +class ExtendFrontMotion : public PublishMotion +{ +public: + ExtendFrontMotion(XmlRpc::XmlRpcValue& motion, ros::Publisher& interface) + : PublishMotion(motion, interface) + { + ROS_ASSERT(motion.hasMember("front")); + target_ = xmlRpcGetDouble(motion, "front", 0.0); + } + bool move() override + { + msg_.data = target_; + return PublishMotion::move(); + } + +private: + double target_; +}; + +class ExtendBackMotion : public PublishMotion +{ +public: + ExtendBackMotion(XmlRpc::XmlRpcValue& motion, ros::Publisher& interface) + : PublishMotion(motion, interface) + { + ROS_ASSERT(motion.hasMember("back")); + target_ = xmlRpcGetDouble(motion, "back", 0.0); + } + bool move() override + { + msg_.data = target_; + return PublishMotion::move(); + } + +private: + double target_; +}; + }; // namespace engineer_middleware diff --git a/engineer_middleware/include/engineer_middleware/step.h b/engineer_middleware/include/engineer_middleware/step.h index b94ea72..00499f9 100644 --- a/engineer_middleware/include/engineer_middleware/step.h +++ b/engineer_middleware/include/engineer_middleware/step.h @@ -91,10 +91,13 @@ class Step ore_lift_motion_ = new JointPointMotion(step["ore_lifter"], ore_lift_pub); if (step.hasMember("gimbal_lifter")) gimbal_lift_motion_ = new JointPointMotion(step["gimbal_lifter"], gimbal_lift_pub); - if (step.hasMember("extend_arm_front")) - extend_arm_front_motion_ = new JointPointMotion(step["extend_arm_front"], extend_arm_f_pub); - if (step.hasMember("extend_arm_back")) - extend_arm_back_motion_ = new JointPointMotion(step["extend_arm_back"], extend_arm_b_pub); + if (step.hasMember("extend_arm")) + { + if (step["extend_arm"].hasMember("front")) + extend_arm_front_motion_ = new ExtendFrontMotion(step["extend_arm"], extend_arm_f_pub); + if (step["extend_arm"].hasMember("back")) + extend_arm_back_motion_ = new ExtendBackMotion(step["extend_arm"], extend_arm_b_pub); + } } bool move() { @@ -203,8 +206,9 @@ class Step MoveitMotionBase* arm_motion_{}; HandMotion* hand_motion_{}; JointPositionMotion* end_effector_motion_{}; - JointPointMotion *ore_rotate_motion_{}, *ore_lift_motion_{}, *gimbal_lift_motion_{}, *extend_arm_front_motion_{}, - *extend_arm_back_motion_{}; + JointPointMotion *ore_rotate_motion_{}, *ore_lift_motion_{}, *gimbal_lift_motion_{}; + ExtendFrontMotion *extend_arm_front_motion_{}; + ExtendBackMotion *extend_arm_back_motion_{}; StoneNumMotion* stone_num_motion_{}; ChassisMotion* chassis_motion_{}; GimbalMotion* gimbal_motion_{}; From ca90f99e6a6c0640e4c1b9bc9949e1a801391e08 Mon Sep 17 00:00:00 2001 From: 2194555 <3631676002@qq.com> Date: Wed, 17 Apr 2024 22:46:26 +0800 Subject: [PATCH 24/25] Update extend_arm logic. --- .../include/engineer_middleware/motion.h | 30 +++++-------------- .../include/engineer_middleware/step.h | 7 ++--- 2 files changed, 10 insertions(+), 27 deletions(-) diff --git a/engineer_middleware/include/engineer_middleware/motion.h b/engineer_middleware/include/engineer_middleware/motion.h index 4a1870b..d5e535d 100644 --- a/engineer_middleware/include/engineer_middleware/motion.h +++ b/engineer_middleware/include/engineer_middleware/motion.h @@ -720,33 +720,17 @@ class JointPointMotion : public PublishMotion double target_; }; -class ExtendFrontMotion : public PublishMotion +class ExtendMotion : public PublishMotion { public: - ExtendFrontMotion(XmlRpc::XmlRpcValue& motion, ros::Publisher& interface) + ExtendMotion(XmlRpc::XmlRpcValue& motion, ros::Publisher& interface, bool is_front) : PublishMotion(motion, interface) { - ROS_ASSERT(motion.hasMember("front")); - target_ = xmlRpcGetDouble(motion, "front", 0.0); - } - bool move() override - { - msg_.data = target_; - return PublishMotion::move(); - } - -private: - double target_; -}; - -class ExtendBackMotion : public PublishMotion -{ -public: - ExtendBackMotion(XmlRpc::XmlRpcValue& motion, ros::Publisher& interface) - : PublishMotion(motion, interface) - { - ROS_ASSERT(motion.hasMember("back")); - target_ = xmlRpcGetDouble(motion, "back", 0.0); + ROS_ASSERT(motion.hasMember("front") || motion.hasMember("back")); + if (is_front) + target_ = xmlRpcGetDouble(motion, "front", 0.0); + else + target_ = xmlRpcGetDouble(motion, "back", 0.0); } bool move() override { diff --git a/engineer_middleware/include/engineer_middleware/step.h b/engineer_middleware/include/engineer_middleware/step.h index 00499f9..253c47c 100644 --- a/engineer_middleware/include/engineer_middleware/step.h +++ b/engineer_middleware/include/engineer_middleware/step.h @@ -94,9 +94,9 @@ class Step if (step.hasMember("extend_arm")) { if (step["extend_arm"].hasMember("front")) - extend_arm_front_motion_ = new ExtendFrontMotion(step["extend_arm"], extend_arm_f_pub); + extend_arm_front_motion_ = new ExtendMotion(step["extend_arm"], extend_arm_f_pub, true); if (step["extend_arm"].hasMember("back")) - extend_arm_back_motion_ = new ExtendBackMotion(step["extend_arm"], extend_arm_b_pub); + extend_arm_back_motion_ = new ExtendMotion(step["extend_arm"], extend_arm_b_pub, false); } } bool move() @@ -207,8 +207,7 @@ class Step HandMotion* hand_motion_{}; JointPositionMotion* end_effector_motion_{}; JointPointMotion *ore_rotate_motion_{}, *ore_lift_motion_{}, *gimbal_lift_motion_{}; - ExtendFrontMotion *extend_arm_front_motion_{}; - ExtendBackMotion *extend_arm_back_motion_{}; + ExtendMotion *extend_arm_front_motion_{}, *extend_arm_back_motion_{}; StoneNumMotion* stone_num_motion_{}; ChassisMotion* chassis_motion_{}; GimbalMotion* gimbal_motion_{}; From 44ecbf84d2e2fbc5ff6eb3bfddf99fd742621263 Mon Sep 17 00:00:00 2001 From: cc0h <7921166012@qq.com> Date: Sat, 20 Apr 2024 17:17:53 +0800 Subject: [PATCH 25/25] fix format. --- engineer_middleware/config/steps_list.yaml | 1 - 1 file changed, 1 deletion(-) diff --git a/engineer_middleware/config/steps_list.yaml b/engineer_middleware/config/steps_list.yaml index 427219b..d3d1a0a 100644 --- a/engineer_middleware/config/steps_list.yaml +++ b/engineer_middleware/config/steps_list.yaml @@ -1349,4 +1349,3 @@ steps_list: - step: "gimbal tall" gimbal_lifter: target: *TALLEST_POS -