From 10ea5834a57add4c91b393e36e17c0822c71547f Mon Sep 17 00:00:00 2001 From: cc0h <7921166012@qq.com> Date: Sun, 14 Apr 2024 22:38:40 +0800 Subject: [PATCH 01/80] Remove drift dimensions. --- .../include/engineer_middleware/motion.h | 33 +++++-------------- 1 file changed, 9 insertions(+), 24 deletions(-) diff --git a/engineer_middleware/include/engineer_middleware/motion.h b/engineer_middleware/include/engineer_middleware/motion.h index c893f49..6369257 100644 --- a/engineer_middleware/include/engineer_middleware/motion.h +++ b/engineer_middleware/include/engineer_middleware/motion.h @@ -236,16 +236,8 @@ class SpaceEeMotion : public EndEffectorMotion tf2_ros::Buffer& tf) : EndEffectorMotion(motion, interface, tf) { - drift_dimensions_.resize(6, true); radius_ = xmlRpcGetDouble(motion, "radius", 0.1); link7_length_ = xmlRpcGetDouble(motion, "link7_length", 0.); - if (motion.hasMember("drift_dimensions")) - { - for (int i = 0; i < (int)drift_dimensions_.size(); ++i) - { - drift_dimensions_[i] = motion["drift_dimensions"][i]; - } - } if (motion.hasMember("basics_length")) { ROS_ASSERT(motion["basics_length"].getType() == XmlRpc::XmlRpcValue::TypeArray); @@ -298,9 +290,6 @@ class SpaceEeMotion : public EndEffectorMotion exchange2base = tf_.lookupTransform("base_link", target_.header.frame_id, ros::Time(0)); quatToRPY(exchange2base.transform.rotation, roll_temp, pitch_temp, yaw_temp); quatToRPY(target_.pose.orientation, roll, pitch, yaw); - roll = drift_dimensions_[3] ? roll : (roll - roll_temp); - pitch = drift_dimensions_[4] ? pitch : (pitch - pitch_temp); - yaw = drift_dimensions_[5] ? yaw : (yaw - yaw_temp); tf2::Quaternion tmp_tf_quaternion; tmp_tf_quaternion.setRPY(roll, pitch, yaw); @@ -317,9 +306,9 @@ class SpaceEeMotion : public EndEffectorMotion temp_target.pose.orientation = quat_tf; tf2::doTransform(temp_target.pose, final_target_.pose, tf_.lookupTransform(interface_.getPlanningFrame(), target_.header.frame_id, ros::Time(0))); - final_target_.pose.position.x = drift_dimensions_[0] ? final_target_.pose.position.x : 0.; - final_target_.pose.position.y = drift_dimensions_[1] ? final_target_.pose.position.y : 0.; - final_target_.pose.position.z = drift_dimensions_[2] ? final_target_.pose.position.z : 0.; + final_target_.pose.position.x = final_target_.pose.position.x; + final_target_.pose.position.y = final_target_.pose.position.y; + final_target_.pose.position.z = final_target_.pose.position.z; final_target_.header.frame_id = interface_.getPlanningFrame(); } catch (tf2::TransformException& ex) @@ -340,19 +329,16 @@ class SpaceEeMotion : public EndEffectorMotion target_.pose.position.z = points_.getPoints()[i].z; quatToRPY(base2exchange.transform.rotation, roll, pitch, yaw); quatToRPY(target_.pose.orientation, roll_temp, pitch_temp, yaw_temp); - roll = drift_dimensions_[3] ? (roll + roll_temp) : 0.; - pitch = drift_dimensions_[4] ? (pitch + pitch_temp) : 0.; - yaw = drift_dimensions_[5] ? (yaw + yaw_temp) : 0.; + roll = roll + roll_temp; + pitch = pitch + pitch_temp; + yaw = yaw + yaw_temp; tf2::Quaternion tf_quaternion; tf_quaternion.setRPY(roll, pitch, yaw); geometry_msgs::Quaternion quat_tf = tf2::toMsg(tf_quaternion); - final_target_.pose.position.x = - drift_dimensions_[0] ? base2exchange.transform.translation.x + target_.pose.position.x : 0.; - final_target_.pose.position.y = - drift_dimensions_[1] ? base2exchange.transform.translation.y + target_.pose.position.y : 0.; - final_target_.pose.position.z = - drift_dimensions_[2] ? base2exchange.transform.translation.z + target_.pose.position.z : 0.; + final_target_.pose.position.x = base2exchange.transform.translation.x + target_.pose.position.x; + final_target_.pose.position.y = base2exchange.transform.translation.y + target_.pose.position.y; + final_target_.pose.position.z = base2exchange.transform.translation.z + target_.pose.position.z; final_target_.pose.orientation = quat_tf; final_target_.header.frame_id = interface_.getPlanningFrame(); } @@ -389,7 +375,6 @@ class SpaceEeMotion : public EndEffectorMotion tolerance_orientation_); } bool is_refer_planning_frame_; - std::vector drift_dimensions_; geometry_msgs::PoseStamped final_target_; int max_planning_times_{}; double radius_, point_resolution_, x_length_, y_length_, z_length_, k_theta_, k_beta_, k_x_, link7_length_; From 89e681edb6d387369c679f38551f7c1a8be4ca47 Mon Sep 17 00:00:00 2001 From: cc0h <7921166012@qq.com> Date: Wed, 17 Apr 2024 22:41:39 +0800 Subject: [PATCH 02/80] Remove points. --- engineer_arm_config/launch/moveit.rviz | 177 +++++++-- engineer_middleware/config/steps_list.yaml | 357 ++++++++++++++++-- .../include/engineer_middleware/motion.h | 167 +++----- 3 files changed, 521 insertions(+), 180 deletions(-) diff --git a/engineer_arm_config/launch/moveit.rviz b/engineer_arm_config/launch/moveit.rviz index ab46ed2..36d800a 100644 --- a/engineer_arm_config/launch/moveit.rviz +++ b/engineer_arm_config/launch/moveit.rviz @@ -84,16 +84,21 @@ Visualization Manager: Alpha: 1 Show Axes: false Show Trail: false - Value: true camera_optical_frame: Alpha: 1 Show Axes: false Show Trail: false - drag_link: + extend_arm_back: Alpha: 1 Show Axes: false Show Trail: false - gimbal_base: + Value: true + extend_arm_front: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + gimbal_lifter: Alpha: 1 Show Axes: false Show Trail: false @@ -143,23 +148,26 @@ Visualization Manager: Show Axes: false Show Trail: false Value: true - mimic_drag_link: + mimic_link1: Alpha: 1 Show Axes: false Show Trail: false - pitch: + Value: true + ore_bin_lifter: Alpha: 1 Show Axes: false Show Trail: false Value: true - pitch_behind_link: + ore_bin_rotator: Alpha: 1 Show Axes: false Show Trail: false - pitch_front_link: + Value: true + pitch: Alpha: 1 Show Axes: false Show Trail: false + Value: true right_back_wheel: Alpha: 1 Show Axes: false @@ -170,19 +178,11 @@ Visualization Manager: Show Axes: false Show Trail: false Value: true - roll_left_link: - Alpha: 1 - Show Axes: false - Show Trail: false - roll_right_link: - Alpha: 1 - Show Axes: false - Show Trail: false tools_link: Alpha: 1 Show Axes: false Show Trail: false - tools_link6: + tools_link_st: Alpha: 1 Show Axes: false Show Trail: false @@ -246,16 +246,21 @@ Visualization Manager: Alpha: 1 Show Axes: false Show Trail: false - Value: true camera_optical_frame: Alpha: 1 Show Axes: false Show Trail: false - drag_link: + extend_arm_back: Alpha: 1 Show Axes: false Show Trail: false - gimbal_base: + Value: true + extend_arm_front: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + gimbal_lifter: Alpha: 1 Show Axes: false Show Trail: false @@ -305,23 +310,26 @@ Visualization Manager: Show Axes: false Show Trail: false Value: true - mimic_drag_link: + mimic_link1: Alpha: 1 Show Axes: false Show Trail: false - pitch: + Value: true + ore_bin_lifter: Alpha: 1 Show Axes: false Show Trail: false Value: true - pitch_behind_link: + ore_bin_rotator: Alpha: 1 Show Axes: false Show Trail: false - pitch_front_link: + Value: true + pitch: Alpha: 1 Show Axes: false Show Trail: false + Value: true right_back_wheel: Alpha: 1 Show Axes: false @@ -332,19 +340,11 @@ Visualization Manager: Show Axes: false Show Trail: false Value: true - roll_left_link: - Alpha: 1 - Show Axes: false - Show Trail: false - roll_right_link: - Alpha: 1 - Show Axes: false - Show Trail: false tools_link: Alpha: 1 Show Axes: false Show Trail: false - tools_link6: + tools_link_st: Alpha: 1 Show Axes: false Show Trail: false @@ -359,12 +359,68 @@ Visualization Manager: Value: true Velocity_Scaling_Factor: 0.1 - Class: rviz/TF - Enabled: false + Enabled: true Filter (blacklist): "" Filter (whitelist): "" Frame Timeout: 15 Frames: All Enabled: false + base_link: + Value: true + camera_link: + Value: true + camera_optical_frame: + Value: true + exchanger: + Value: true + extend_arm_back: + Value: true + extend_arm_front: + Value: true + gimbal_lifter: + Value: true + left_back_wheel: + Value: true + left_front_wheel: + Value: true + link1: + Value: true + link2: + Value: true + link3: + Value: true + link4: + Value: true + link5: + Value: true + link6: + Value: true + link7: + Value: true + map: + Value: true + mimic_link1: + Value: true + odom: + Value: true + ore_bin_lifter: + Value: true + ore_bin_rotator: + Value: true + pitch: + Value: true + real_world: + Value: true + right_back_wheel: + Value: true + right_front_wheel: + Value: true + tools_link: + Value: true + tools_link_st: + Value: true + yaw: + Value: true Marker Alpha: 1 Marker Scale: 1 Name: TF @@ -372,9 +428,52 @@ Visualization Manager: Show Axes: true Show Names: true Tree: - {} + map: + exchanger: + {} + odom: + base_link: + extend_arm_back: + {} + extend_arm_front: + {} + gimbal_lifter: + camera_link: + {} + camera_optical_frame: + {} + pitch: + {} + yaw: + {} + left_back_wheel: + {} + left_front_wheel: + {} + link1: + link2: + link3: + link4: + link5: + link6: + link7: + tools_link: + {} + tools_link_st: + {} + mimic_link1: + {} + ore_bin_rotator: + ore_bin_lifter: + {} + real_world: + {} + right_back_wheel: + {} + right_front_wheel: + {} Update Interval: 0 - Value: false + Value: true Enabled: true Global Options: Background Color: 48; 48; 48 @@ -391,7 +490,7 @@ Visualization Manager: Views: Current: Class: rviz/Orbit - Distance: 0.8833990097045898 + Distance: 3.3441882133483887 Enable Stereo Rendering: Stereo Eye Separation: 0.05999999865889549 Stereo Focal Distance: 1 @@ -407,9 +506,9 @@ Visualization Manager: Invert Z Axis: false Name: Current View Near Clip Distance: 0.009999999776482582 - Pitch: 0.5947970747947693 + Pitch: 0.20979738235473633 Target Frame: base_link - Yaw: 1.4263750314712524 + Yaw: 3.1727137565612793 Saved: ~ Window Geometry: Displays: @@ -428,4 +527,4 @@ Window Geometry: collapsed: false Width: 1665 X: 757 - Y: 84 + Y: 27 diff --git a/engineer_middleware/config/steps_list.yaml b/engineer_middleware/config/steps_list.yaml index d485ff8..018f06c 100644 --- a/engineer_middleware/config/steps_list.yaml +++ b/engineer_middleware/config/steps_list.yaml @@ -187,13 +187,14 @@ common: small_island: &LIFTER_SMALL_ISLAND 0.1 mid_pos: &MID_POS - 0.14 + 0.1 big_island: &LIFTER_BIG_ISLAND - 0.260 + + 0.14 two_stone_pos: &TWO_STONE_POS 0.163 tallest_pos: &TALLEST_POS - 0.394 + 0.28 gripper: open_gripper: &OPEN_GRIPPER @@ -285,10 +286,8 @@ 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 @@ -311,10 +310,8 @@ 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 @@ -337,10 +334,8 @@ 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 @@ -365,7 +360,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 @@ -386,7 +382,7 @@ steps_list: - 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 ] @@ -463,6 +459,101 @@ steps_list: gimbal: <<: *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 AND BACK" + arm: + 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_STONE2, *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" @@ -734,16 +825,15 @@ steps_list: change: "-1" #################BIG_ISLAND############# - MID_BIG_ISLAND: + ######MID_STONE####### + HAS0_MID_BIG_ISLAND: - step: "gimbal ready" gimbal: <<: *SMALL_ISLAND_POS - step: "open ex arm" - extend_arm_front: - target: *EX_OPEN - - 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 ] @@ -763,7 +853,7 @@ steps_list: - step: "OPEN_GRIPPER" gripper: <<: *OPEN_GRIPPER - MID_BIG_ISLAND0: + 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 ] @@ -843,17 +933,117 @@ steps_list: gimbal: <<: *FRONT_POS - - SIDE_BIG_ISLAND: + HAS1_MID_BIG_ISLAND: - step: "gimbal ready" gimbal: <<: *SMALL_ISLAND_POS - step: "open ex arm" - extend_arm_front: - target: *EX_OPEN -# - 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 ] + 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 + - 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 ] @@ -873,7 +1063,7 @@ steps_list: - step: "OPEN_GRIPPER" gripper: <<: *OPEN_GRIPPER - SIDE_BIG_ISLAND0: + 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 ] @@ -953,6 +1143,109 @@ steps_list: 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 ########################################################## ########## EXCHANGE ############ diff --git a/engineer_middleware/include/engineer_middleware/motion.h b/engineer_middleware/include/engineer_middleware/motion.h index 6369257..a84783a 100644 --- a/engineer_middleware/include/engineer_middleware/motion.h +++ b/engineer_middleware/include/engineer_middleware/motion.h @@ -48,7 +48,6 @@ #include #include #include -#include namespace engineer_middleware { @@ -113,17 +112,12 @@ class MoveitMotionBase : public MotionBase Date: Wed, 17 Apr 2024 23:30:55 +0800 Subject: [PATCH 03/80] Remove points and make space ee motion available. --- engineer_arm_config/launch/moveit.rviz | 62 ++++++++-------- engineer_middleware/config/steps_list.yaml | 27 ------- .../include/engineer_middleware/middleware.h | 3 +- .../include/engineer_middleware/motion.h | 72 +++++++++++-------- .../include/engineer_middleware/step.h | 11 +-- .../include/engineer_middleware/step_queue.h | 9 ++- engineer_middleware/src/middleware.cpp | 5 +- 7 files changed, 84 insertions(+), 105 deletions(-) diff --git a/engineer_arm_config/launch/moveit.rviz b/engineer_arm_config/launch/moveit.rviz index 36d800a..bd3c630 100644 --- a/engineer_arm_config/launch/moveit.rviz +++ b/engineer_arm_config/launch/moveit.rviz @@ -368,59 +368,59 @@ Visualization Manager: base_link: Value: true camera_link: - Value: true + Value: false camera_optical_frame: - Value: true + Value: false exchanger: Value: true extend_arm_back: - Value: true + Value: false extend_arm_front: - Value: true + Value: false gimbal_lifter: - Value: true + Value: false left_back_wheel: - Value: true + Value: false left_front_wheel: - Value: true + Value: false link1: - Value: true + Value: false link2: - Value: true + Value: false link3: - Value: true + Value: false link4: - Value: true + Value: false link5: - Value: true + Value: false link6: - Value: true + Value: false link7: Value: true map: - Value: true + Value: false mimic_link1: - Value: true + Value: false odom: - Value: true + Value: false ore_bin_lifter: - Value: true + Value: false ore_bin_rotator: - Value: true + Value: false pitch: - Value: true + Value: false real_world: - Value: true + Value: false right_back_wheel: - Value: true + Value: false right_front_wheel: - Value: true + Value: false tools_link: - Value: true + Value: false tools_link_st: - Value: true + Value: false yaw: - Value: true + Value: false Marker Alpha: 1 Marker Scale: 1 Name: TF @@ -490,7 +490,7 @@ Visualization Manager: Views: Current: Class: rviz/Orbit - Distance: 3.3441882133483887 + Distance: 2.504984140396118 Enable Stereo Rendering: Stereo Eye Separation: 0.05999999865889549 Stereo Focal Distance: 1 @@ -498,17 +498,17 @@ Visualization Manager: Value: false Field of View: 0.75 Focal Point: - X: 0.7602222561836243 - Y: -0.15494105219841003 - Z: 0.9399657249450684 + X: 0.23874804377555847 + Y: 0.04174364358186722 + Z: 0.6224369406700134 Focal Shape Fixed Size: true Focal Shape Size: 0.05000000074505806 Invert Z Axis: false Name: Current View Near Clip Distance: 0.009999999776482582 - Pitch: 0.20979738235473633 + Pitch: 0.5297974348068237 Target Frame: base_link - Yaw: 3.1727137565612793 + Yaw: 2.0677237510681152 Saved: ~ Window Geometry: Displays: diff --git a/engineer_middleware/config/steps_list.yaml b/engineer_middleware/config/steps_list.yaml index 018f06c..f87bd8e 100644 --- a/engineer_middleware/config/steps_list.yaml +++ b/engineer_middleware/config/steps_list.yaml @@ -284,10 +284,6 @@ steps_list: - step: "gimbal lifter down" gimbal_lifter: target: *BUTTON_POS - - step: "close ex arm" - extend_arm_front: - front: *EX_CLOSE - back: *EX_CLOSE - step: "close gripper" gripper: <<: *CLOSE_GRIPPER @@ -308,10 +304,6 @@ steps_list: - step: "gimbal lifter down" gimbal_lifter: target: *MID_POS - - step: "close ex arm" - extend_arm_front: - front: *EX_CLOSE - back: *EX_CLOSE - step: "close gripper" gripper: <<: *CLOSE_GRIPPER @@ -332,10 +324,6 @@ steps_list: - step: "gimbal lifter down" gimbal_lifter: target: *MID_POS - - step: "close ex arm" - extend_arm_front: - front: *EX_CLOSE - back: *EX_CLOSE - step: "close gripper" gripper: <<: *CLOSE_GRIPPER @@ -830,10 +818,6 @@ steps_list: - 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 ] @@ -937,10 +921,6 @@ steps_list: - 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 ] @@ -1041,9 +1021,6 @@ steps_list: - 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 ] @@ -1147,9 +1124,6 @@ steps_list: - 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 ] @@ -1321,7 +1295,6 @@ steps_list: 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 diff --git a/engineer_middleware/include/engineer_middleware/middleware.h b/engineer_middleware/include/engineer_middleware/middleware.h index cc9b809..439f8f3 100644 --- a/engineer_middleware/include/engineer_middleware/middleware.h +++ b/engineer_middleware/include/engineer_middleware/middleware.h @@ -82,8 +82,7 @@ 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_, extend_arm_f_pub_, - extend_arm_b_pub_; + stone_num_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/motion.h b/engineer_middleware/include/engineer_middleware/motion.h index a84783a..74403bb 100644 --- a/engineer_middleware/include/engineer_middleware/motion.h +++ b/engineer_middleware/include/engineer_middleware/motion.h @@ -230,7 +230,6 @@ class SpaceEeMotion : public EndEffectorMotion tf2_ros::Buffer& tf) : EndEffectorMotion(motion, interface, tf) { - max_planning_times_ = (int)xmlRpcGetDouble(motion, "max_planning_times", 3); if (motion.hasMember("is_refer_planning_frame")) is_refer_planning_frame_ = motion["is_refer_planning_frame"]; else @@ -243,33 +242,40 @@ class SpaceEeMotion : public EndEffectorMotion { if (!is_refer_planning_frame_) { - try - { - double roll, pitch, yaw, roll_temp, pitch_temp, yaw_temp; - geometry_msgs::TransformStamped exchange2base; - exchange2base = tf_.lookupTransform("base_link", target_.header.frame_id, ros::Time(0)); - quatToRPY(exchange2base.transform.rotation, roll_temp, pitch_temp, yaw_temp); - quatToRPY(target_.pose.orientation, roll, pitch, yaw); - - tf2::Quaternion tmp_tf_quaternion; - tmp_tf_quaternion.setRPY(roll, pitch, yaw); - geometry_msgs::Quaternion quat_tf = tf2::toMsg(tmp_tf_quaternion); - quatToRPY(quat_tf, roll, pitch, yaw); - geometry_msgs::PoseStamped temp_target; - temp_target.pose.position = target_.pose.position; - temp_target.pose.orientation = quat_tf; - tf2::doTransform(temp_target.pose, final_target_.pose, - tf_.lookupTransform(interface_.getPlanningFrame(), target_.header.frame_id, ros::Time(0))); - final_target_.pose.position.x = final_target_.pose.position.x; - final_target_.pose.position.y = final_target_.pose.position.y; - final_target_.pose.position.z = final_target_.pose.position.z; - final_target_.header.frame_id = interface_.getPlanningFrame(); - } - catch (tf2::TransformException& ex) - { - ROS_WARN("%s", ex.what()); - return false; - } + // try + // { + // double roll, pitch, yaw, roll_temp, pitch_temp, yaw_temp; + // geometry_msgs::TransformStamped exchange2base; + // exchange2base = tf_.lookupTransform("base_link", target_.header.frame_id, ros::Time(0)); + // quatToRPY(exchange2base.transform.rotation, roll_temp, pitch_temp, yaw_temp); + // quatToRPY(target_.pose.orientation, roll, pitch, yaw); + // + // tf2::Quaternion tmp_tf_quaternion; + // tmp_tf_quaternion.setRPY(roll, pitch, yaw); + // geometry_msgs::Quaternion quat_tf = tf2::toMsg(tmp_tf_quaternion); + // quatToRPY(quat_tf, roll, pitch, yaw); + // points_.rectifyForRPY(pitch_temp, yaw_temp, k_x_, k_theta_, k_beta_); + // if (link7_length_) + // points_.rectifyForLink7(pitch_temp, link7_length_); + // target_.pose.position.x = points_.getPoints()[i].x; + // target_.pose.position.y = points_.getPoints()[i].y; + // target_.pose.position.z = points_.getPoints()[i].z; + // geometry_msgs::PoseStamped temp_target; + // temp_target.pose.position = target_.pose.position; + // temp_target.pose.orientation = quat_tf; + // tf2::doTransform(temp_target.pose, final_target_.pose, + // tf_.lookupTransform(interface_.getPlanningFrame(), target_.header.frame_id, + // ros::Time(0))); + // final_target_.pose.position.x = final_target_.pose.position.x; + // final_target_.pose.position.y = final_target_.pose.position.y; + // final_target_.pose.position.z = final_target_.pose.position.z; + // final_target_.header.frame_id = interface_.getPlanningFrame(); + // } + // catch (tf2::TransformException& ex) + // { + // ROS_WARN("%s", ex.what()); + // return false; + // } } else { @@ -278,11 +284,17 @@ class SpaceEeMotion : public EndEffectorMotion double roll, pitch, yaw, roll_temp, pitch_temp, yaw_temp; geometry_msgs::TransformStamped base2exchange; base2exchange = tf_.lookupTransform("base_link", target_.header.frame_id, ros::Time(0)); + target_.pose.position.x = target_.pose.position.x; + target_.pose.position.y = target_.pose.position.y; + target_.pose.position.z = target_.pose.position.z; quatToRPY(base2exchange.transform.rotation, roll, pitch, yaw); + ROS_INFO_STREAM("base2exchange roll: " << roll); quatToRPY(target_.pose.orientation, roll_temp, pitch_temp, yaw_temp); + ROS_INFO_STREAM("target roll: " << roll_temp); roll = roll + roll_temp; pitch = pitch + pitch_temp; yaw = yaw + yaw_temp; + ROS_INFO_STREAM("roll added: " << roll); tf2::Quaternion tf_quaternion; tf_quaternion.setRPY(roll, pitch, yaw); geometry_msgs::Quaternion quat_tf = tf2::toMsg(tf_quaternion); @@ -292,6 +304,9 @@ class SpaceEeMotion : public EndEffectorMotion final_target_.pose.position.z = base2exchange.transform.translation.z + target_.pose.position.z; final_target_.pose.orientation = quat_tf; final_target_.header.frame_id = interface_.getPlanningFrame(); + double rolll, pitchh, yaww; + quatToRPY(final_target_.pose.orientation, rolll, pitchh, yaww); + ROS_INFO_STREAM("transformed roll: " << rolll); } catch (tf2::TransformException& ex) { @@ -326,7 +341,6 @@ class SpaceEeMotion : public EndEffectorMotion } bool is_refer_planning_frame_; geometry_msgs::PoseStamped final_target_; - int max_planning_times_{}; }; class JointMotion : public MoveitMotionBase diff --git a/engineer_middleware/include/engineer_middleware/step.h b/engineer_middleware/include/engineer_middleware/step.h index b94ea72..f21ff9e 100644 --- a/engineer_middleware/include/engineer_middleware/step.h +++ b/engineer_middleware/include/engineer_middleware/step.h @@ -50,9 +50,9 @@ class Step moveit::planning_interface::MoveGroupInterface& arm_group, ChassisInterface& chassis_interface, 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& 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::Publisher& ore_rotate_pub, ros::Publisher& ore_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), arm_group_(arm_group) { ROS_ASSERT(step.hasMember("step")); step_name_ = static_cast(step["step"]); @@ -102,11 +102,6 @@ class Step if (arm_motion_) { success &= arm_motion_->move(); - if (!arm_motion_->getPointCloud2().data.empty()) - { - sensor_msgs::PointCloud2 point_cloud2 = arm_motion_->getPointCloud2(); - point_cloud_pub_.publish(point_cloud2); - } std_msgs::Int32 msg = arm_motion_->getPlanningResult(); planning_result_pub_.publish(msg); } diff --git a/engineer_middleware/include/engineer_middleware/step_queue.h b/engineer_middleware/include/engineer_middleware/step_queue.h index d2fa912..c020932 100644 --- a/engineer_middleware/include/engineer_middleware/step_queue.h +++ b/engineer_middleware/include/engineer_middleware/step_queue.h @@ -55,16 +55,15 @@ class StepQueue moveit::planning_interface::MoveGroupInterface& arm_group, ChassisInterface& chassis_interface, 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& extend_arm_f_pub, - ros::Publisher& extend_arm_b_pub) + ros::Publisher& planning_result_pub, ros::Publisher& ore_rotate_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, extend_arm_f_pub, extend_arm_b_pub); + gimbal_pub, gpio_pub, reversal_pub, planning_result_pub, ore_rotate_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 3ef4368..a27b7a1 100644 --- a/engineer_middleware/src/middleware.cpp +++ b/engineer_middleware/src/middleware.cpp @@ -52,7 +52,6 @@ Middleware::Middleware(ros::NodeHandle& nh) , reversal_pub_(nh.advertise("/controllers/multi_dof_controller/command", 10)) , planning_result_pub_(nh.advertise("/planning_result", 10)) , stone_num_pub_(nh.advertise("/stone_num", 10)) - , 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)) @@ -74,8 +73,8 @@ Middleware::Middleware(ros::NodeHandle& nh) step_queues_.insert( 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_, extend_arm_f_pub_, extend_arm_b_pub_))); + planning_result_pub_, ore_rotate_pub_, ore_lift_pub_, gimbal_lift_pub_, + extend_arm_f_pub_, extend_arm_b_pub_))); } } else From 9ddcfb05024e55fe922581bda86afee929606858 Mon Sep 17 00:00:00 2001 From: cc0h <7921166012@qq.com> Date: Thu, 18 Apr 2024 22:27:56 +0800 Subject: [PATCH 04/80] record. --- engineer_arm_config/launch/moveit.rviz | 102 ++++++++++-------- engineer_middleware/config/steps_list.yaml | 3 - .../include/engineer_middleware/motion.h | 36 +++++-- 3 files changed, 85 insertions(+), 56 deletions(-) diff --git a/engineer_arm_config/launch/moveit.rviz b/engineer_arm_config/launch/moveit.rviz index bd3c630..a0a4838 100644 --- a/engineer_arm_config/launch/moveit.rviz +++ b/engineer_arm_config/launch/moveit.rviz @@ -428,50 +428,60 @@ Visualization Manager: Show Axes: true Show Names: true Tree: - map: - exchanger: + base_link: + {} + exchanger: + {} + extend_arm_back: + {} + extend_arm_front: + {} + gimbal_lifter: + camera_link: + {} + camera_optical_frame: + {} + left_back_wheel: + {} + left_front_wheel: + {} + link1: + {} + link2: + {} + link3: + {} + link4: + {} + link5: + {} + link6: + {} + link7: + tools_link: {} - odom: - base_link: - extend_arm_back: - {} - extend_arm_front: - {} - gimbal_lifter: - camera_link: - {} - camera_optical_frame: - {} - pitch: - {} - yaw: - {} - left_back_wheel: - {} - left_front_wheel: - {} - link1: - link2: - link3: - link4: - link5: - link6: - link7: - tools_link: - {} - tools_link_st: - {} - mimic_link1: - {} - ore_bin_rotator: - ore_bin_lifter: - {} - real_world: - {} - right_back_wheel: - {} - right_front_wheel: - {} + tools_link_st: + {} + map: + {} + mimic_link1: + {} + odom: + {} + ore_bin_lifter: + {} + ore_bin_rotator: + {} + pitch: + {} + real_world: + {} + right_back_wheel: + {} + right_front_wheel: + {} + yaw: + {} Update Interval: 0 Value: true Enabled: true @@ -490,7 +500,7 @@ Visualization Manager: Views: Current: Class: rviz/Orbit - Distance: 2.504984140396118 + Distance: 1.4108070135116577 Enable Stereo Rendering: Stereo Eye Separation: 0.05999999865889549 Stereo Focal Distance: 1 @@ -506,9 +516,9 @@ Visualization Manager: Invert Z Axis: false Name: Current View Near Clip Distance: 0.009999999776482582 - Pitch: 0.5297974348068237 + Pitch: 0.48979732394218445 Target Frame: base_link - Yaw: 2.0677237510681152 + Yaw: 3.2827558517456055 Saved: ~ Window Geometry: Displays: diff --git a/engineer_middleware/config/steps_list.yaml b/engineer_middleware/config/steps_list.yaml index f87bd8e..49318cc 100644 --- a/engineer_middleware/config/steps_list.yaml +++ b/engineer_middleware/config/steps_list.yaml @@ -1250,9 +1250,6 @@ steps_list: 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 diff --git a/engineer_middleware/include/engineer_middleware/motion.h b/engineer_middleware/include/engineer_middleware/motion.h index 74403bb..8aef38c 100644 --- a/engineer_middleware/include/engineer_middleware/motion.h +++ b/engineer_middleware/include/engineer_middleware/motion.h @@ -291,18 +291,39 @@ class SpaceEeMotion : public EndEffectorMotion ROS_INFO_STREAM("base2exchange roll: " << roll); quatToRPY(target_.pose.orientation, roll_temp, pitch_temp, yaw_temp); ROS_INFO_STREAM("target roll: " << roll_temp); - roll = roll + roll_temp; - pitch = pitch + pitch_temp; - yaw = yaw + yaw_temp; - ROS_INFO_STREAM("roll added: " << roll); + quat_base2exchange_.setW(base2exchange.transform.rotation.w); + quat_base2exchange_.setX(base2exchange.transform.rotation.x); + quat_base2exchange_.setY(base2exchange.transform.rotation.y); + quat_base2exchange_.setZ(base2exchange.transform.rotation.z); + ROS_INFO_STREAM("base2exchange quat w: " << quat_base2exchange_.w()); + ROS_INFO_STREAM("base2exchange quat x: " << quat_base2exchange_.x()); + ROS_INFO_STREAM("base2exchange quat y: " << quat_base2exchange_.y()); + ROS_INFO_STREAM("base2exchange quat z: " << quat_base2exchange_.z()); + + quat_target_.setW(target_.pose.orientation.w); + quat_target_.setX(target_.pose.orientation.x); + quat_target_.setY(target_.pose.orientation.y); + quat_target_.setZ(target_.pose.orientation.z); + ROS_INFO_STREAM("target quat w: " << quat_target_.w()); + ROS_INFO_STREAM("target quat x: " << quat_target_.x()); + ROS_INFO_STREAM("target quat y: " << quat_target_.y()); + ROS_INFO_STREAM("target quat z: " << quat_target_.z()); + tf2::Quaternion tf_quaternion; - tf_quaternion.setRPY(roll, pitch, yaw); - geometry_msgs::Quaternion quat_tf = tf2::toMsg(tf_quaternion); + tf_quaternion = quat_target_ * quat_base2exchange_; final_target_.pose.position.x = base2exchange.transform.translation.x + target_.pose.position.x; final_target_.pose.position.y = base2exchange.transform.translation.y + target_.pose.position.y; final_target_.pose.position.z = base2exchange.transform.translation.z + target_.pose.position.z; - final_target_.pose.orientation = quat_tf; + final_target_.pose.orientation.w = tf_quaternion.w(); + final_target_.pose.orientation.x = tf_quaternion.x(); + final_target_.pose.orientation.y = tf_quaternion.y(); + final_target_.pose.orientation.z = tf_quaternion.z(); + ROS_INFO_STREAM("final target w: " << final_target_.pose.orientation.w); + ROS_INFO_STREAM("final target x: " << final_target_.pose.orientation.x); + ROS_INFO_STREAM("final target y: " << final_target_.pose.orientation.y); + ROS_INFO_STREAM("final target z: " << final_target_.pose.orientation.z); + final_target_.header.frame_id = interface_.getPlanningFrame(); double rolll, pitchh, yaww; quatToRPY(final_target_.pose.orientation, rolll, pitchh, yaww); @@ -341,6 +362,7 @@ class SpaceEeMotion : public EndEffectorMotion } bool is_refer_planning_frame_; geometry_msgs::PoseStamped final_target_; + tf2::Quaternion quat_base2exchange_, quat_target_; }; class JointMotion : public MoveitMotionBase From 86c5ecce73d0e5c115adb7967a345442e4a84177 Mon Sep 17 00:00:00 2001 From: cc0h <7921166012@qq.com> Date: Sat, 20 Apr 2024 17:10:00 +0800 Subject: [PATCH 05/80] modify quaternion multiplication order. --- engineer_middleware/config/steps_list.yaml | 20 +++---------------- .../include/engineer_middleware/motion.h | 2 +- 2 files changed, 4 insertions(+), 18 deletions(-) diff --git a/engineer_middleware/config/steps_list.yaml b/engineer_middleware/config/steps_list.yaml index 49318cc..d236696 100644 --- a/engineer_middleware/config/steps_list.yaml +++ b/engineer_middleware/config/steps_list.yaml @@ -1245,7 +1245,7 @@ steps_list: arm: frame: exchanger is_refer_planning_frame: true - xyz: [ -0.3, 0.2, -0.2 ] + xyz: [ -0.2, 0., 0. ] rpy: [ 0., -1.5707963, 0. ] drift_dimensions: [ true, true, true, true, true, true ] tolerance_position: 0.01 @@ -1270,27 +1270,13 @@ steps_list: 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. ] + xyz: [ -0.2, 0., 0. ] rpy: [ 0., 3.1415926, 0. ] tolerance_position: 0.01 tolerance_orientation: 0.001 diff --git a/engineer_middleware/include/engineer_middleware/motion.h b/engineer_middleware/include/engineer_middleware/motion.h index 8aef38c..dd5b48f 100644 --- a/engineer_middleware/include/engineer_middleware/motion.h +++ b/engineer_middleware/include/engineer_middleware/motion.h @@ -310,7 +310,7 @@ class SpaceEeMotion : public EndEffectorMotion ROS_INFO_STREAM("target quat z: " << quat_target_.z()); tf2::Quaternion tf_quaternion; - tf_quaternion = quat_target_ * quat_base2exchange_; + tf_quaternion = quat_base2exchange_ * quat_target_; final_target_.pose.position.x = base2exchange.transform.translation.x + target_.pose.position.x; final_target_.pose.position.y = base2exchange.transform.translation.y + target_.pose.position.y; From 9fd7f4195d1cad7159a126b96bf30e7ed85d1c53 Mon Sep 17 00:00:00 2001 From: cc0h <7921166012@qq.com> Date: Sat, 20 Apr 2024 17:17:31 +0800 Subject: [PATCH 06/80] 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 - From 11e44202501b32700069feb76e3ce58cb796a164 Mon Sep 17 00:00:00 2001 From: cc0h <7921166012@qq.com> Date: Sat, 20 Apr 2024 20:22:08 +0800 Subject: [PATCH 07/80] Available auto exchange. --- .../include/engineer_middleware/motion.h | 14 -------------- 1 file changed, 14 deletions(-) diff --git a/engineer_middleware/include/engineer_middleware/motion.h b/engineer_middleware/include/engineer_middleware/motion.h index dd5b48f..e237922 100644 --- a/engineer_middleware/include/engineer_middleware/motion.h +++ b/engineer_middleware/include/engineer_middleware/motion.h @@ -288,26 +288,16 @@ class SpaceEeMotion : public EndEffectorMotion target_.pose.position.y = target_.pose.position.y; target_.pose.position.z = target_.pose.position.z; quatToRPY(base2exchange.transform.rotation, roll, pitch, yaw); - ROS_INFO_STREAM("base2exchange roll: " << roll); quatToRPY(target_.pose.orientation, roll_temp, pitch_temp, yaw_temp); - ROS_INFO_STREAM("target roll: " << roll_temp); quat_base2exchange_.setW(base2exchange.transform.rotation.w); quat_base2exchange_.setX(base2exchange.transform.rotation.x); quat_base2exchange_.setY(base2exchange.transform.rotation.y); quat_base2exchange_.setZ(base2exchange.transform.rotation.z); - ROS_INFO_STREAM("base2exchange quat w: " << quat_base2exchange_.w()); - ROS_INFO_STREAM("base2exchange quat x: " << quat_base2exchange_.x()); - ROS_INFO_STREAM("base2exchange quat y: " << quat_base2exchange_.y()); - ROS_INFO_STREAM("base2exchange quat z: " << quat_base2exchange_.z()); quat_target_.setW(target_.pose.orientation.w); quat_target_.setX(target_.pose.orientation.x); quat_target_.setY(target_.pose.orientation.y); quat_target_.setZ(target_.pose.orientation.z); - ROS_INFO_STREAM("target quat w: " << quat_target_.w()); - ROS_INFO_STREAM("target quat x: " << quat_target_.x()); - ROS_INFO_STREAM("target quat y: " << quat_target_.y()); - ROS_INFO_STREAM("target quat z: " << quat_target_.z()); tf2::Quaternion tf_quaternion; tf_quaternion = quat_base2exchange_ * quat_target_; @@ -319,10 +309,6 @@ class SpaceEeMotion : public EndEffectorMotion final_target_.pose.orientation.x = tf_quaternion.x(); final_target_.pose.orientation.y = tf_quaternion.y(); final_target_.pose.orientation.z = tf_quaternion.z(); - ROS_INFO_STREAM("final target w: " << final_target_.pose.orientation.w); - ROS_INFO_STREAM("final target x: " << final_target_.pose.orientation.x); - ROS_INFO_STREAM("final target y: " << final_target_.pose.orientation.y); - ROS_INFO_STREAM("final target z: " << final_target_.pose.orientation.z); final_target_.header.frame_id = interface_.getPlanningFrame(); double rolll, pitchh, yaww; From 016e7bedf3eefac59503b294b9c2e8863f8f0972 Mon Sep 17 00:00:00 2001 From: cc0h <7921166012@qq.com> Date: Sun, 21 Apr 2024 18:39:20 +0800 Subject: [PATCH 08/80] Add points. --- .../include/engineer_middleware/middleware.h | 3 +- .../include/engineer_middleware/motion.h | 182 ++++++++++-------- .../include/engineer_middleware/step.h | 11 +- .../include/engineer_middleware/step_queue.h | 9 +- engineer_middleware/src/middleware.cpp | 5 +- 5 files changed, 124 insertions(+), 86 deletions(-) diff --git a/engineer_middleware/include/engineer_middleware/middleware.h b/engineer_middleware/include/engineer_middleware/middleware.h index 439f8f3..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_, ore_rotate_pub_, ore_lift_pub_, gimbal_lift_pub_, extend_arm_f_pub_, extend_arm_b_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/motion.h b/engineer_middleware/include/engineer_middleware/motion.h index c8038e1..44cd7f9 100644 --- a/engineer_middleware/include/engineer_middleware/motion.h +++ b/engineer_middleware/include/engineer_middleware/motion.h @@ -48,6 +48,7 @@ #include #include #include +#include namespace engineer_middleware { @@ -112,12 +113,17 @@ class MoveitMotionBase : public MotionBase(step["step"]); @@ -105,6 +105,11 @@ class Step if (arm_motion_) { success &= arm_motion_->move(); + if (!arm_motion_->getPointCloud2().data.empty()) + { + sensor_msgs::PointCloud2 point_cloud2 = arm_motion_->getPointCloud2(); + point_cloud_pub_.publish(point_cloud2); + } std_msgs::Int32 msg = arm_motion_->getPlanningResult(); planning_result_pub_.publish(msg); } diff --git a/engineer_middleware/include/engineer_middleware/step_queue.h b/engineer_middleware/include/engineer_middleware/step_queue.h index c020932..d2fa912 100644 --- a/engineer_middleware/include/engineer_middleware/step_queue.h +++ b/engineer_middleware/include/engineer_middleware/step_queue.h @@ -55,15 +55,16 @@ class StepQueue moveit::planning_interface::MoveGroupInterface& arm_group, ChassisInterface& chassis_interface, 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& ore_rotate_pub, ros::Publisher& ore_lift_pub, - ros::Publisher& gimbal_lift_pub, ros::Publisher& extend_arm_f_pub, ros::Publisher& extend_arm_b_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& 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, ore_rotate_pub, ore_lift_pub, - gimbal_lift_pub, extend_arm_f_pub, extend_arm_b_pub); + gimbal_pub, gpio_pub, reversal_pub, planning_result_pub, point_cloud_pub, ore_rotate_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 a27b7a1..3ef4368 100644 --- a/engineer_middleware/src/middleware.cpp +++ b/engineer_middleware/src/middleware.cpp @@ -52,6 +52,7 @@ Middleware::Middleware(ros::NodeHandle& nh) , reversal_pub_(nh.advertise("/controllers/multi_dof_controller/command", 10)) , planning_result_pub_(nh.advertise("/planning_result", 10)) , stone_num_pub_(nh.advertise("/stone_num", 10)) + , 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)) @@ -73,8 +74,8 @@ Middleware::Middleware(ros::NodeHandle& nh) step_queues_.insert( 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_, ore_rotate_pub_, ore_lift_pub_, gimbal_lift_pub_, - extend_arm_f_pub_, extend_arm_b_pub_))); + planning_result_pub_, point_cloud_pub_, ore_rotate_pub_, ore_lift_pub_, + gimbal_lift_pub_, extend_arm_f_pub_, extend_arm_b_pub_))); } } else From 6e22eea2b041f6bbec50e0c3c0fd262edb37026c Mon Sep 17 00:00:00 2001 From: cc0h <7921166012@qq.com> Date: Sun, 21 Apr 2024 18:39:45 +0800 Subject: [PATCH 09/80] Update steps list. --- engineer_middleware/config/steps_list.yaml | 522 ++++++++++++++------- 1 file changed, 365 insertions(+), 157 deletions(-) diff --git a/engineer_middleware/config/steps_list.yaml b/engineer_middleware/config/steps_list.yaml index d236696..96a1678 100644 --- a/engineer_middleware/config/steps_list.yaml +++ b/engineer_middleware/config/steps_list.yaml @@ -28,11 +28,15 @@ common: down_position: &JOINT1_DOWN_POSITION 0.02 small_ready: &JOINT1_SMALL_READY - 0.16 + 0.475 + small_down: &JOINT1_SMALL_DOWN + 0.31 + small_ready_store: &JOINT1_SMALL_READY_STORE + 0.249 + small_ready_store_third: &JOINT1_SMALL_READY_STORE_THIRD + 0.387 up_pos: &JOINT1_UP_POS 0.312 - small_ready_store: &JOINT1_SMALL_READY_STORE - 0.17 bin_get_stone: &JOINT1_BIN_GET_STONE 0.142 big_ready: &JOINT1_BIG_READY @@ -63,12 +67,8 @@ common: 0.232 exchange_position: &JOINT2_EXCHANGE_POSITION 0.07 - small_arm_up: &JOINT2_SMALL_ARM_UP - 0.085 - small_store_stone: &JOINT2_SMALL_STORE_STONE - 0.02 - small_ready_second: &JOINT2_SMALL_READY_SECOND - 0.142 + small_ready: &JOINT2_SMALL_READY + 0.025 bin_get_stone: &JOINT2_BIN_GET_STONE 0.087 back_bin_position: &JOINT2_BACK_BIN_POSITION @@ -90,18 +90,14 @@ common: -0.275 home: &JOINT3_HOME_POS 0.08 - small_ready: &JOINT3_SMALL_READY - 0.208 - small_ready_second: &JOINT3_SMALL_READY_SECOND - 0.03 - small_get_stone: &JOINT3_SMALL_GET_STONE - 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.291 + small_ready_mid: &JOINT3_SMALL_READY_MID + 0.037 + small_ready_left: &JOINT3_SMALL_READY_LEFT + 0.308 + small_ready_right: &JOINT3_SMALL_READY_RIGHT + -0.233 + small_turn: &JOINT3_SMALL_TURN + 0.257 bin_get_stone: &JOINT3_BIN_GET_STONE 0.258 big_ready: &JOINT3_BIG_READY @@ -123,8 +119,10 @@ common: 0.001 right_position: &JOINT4_R_POSITION -1.495 - small_ready_second: &JOINT4_SMALL_READY_SECOND - -0.777 + small_ready: &JOINT4_SMALL_READY + 0.054 + small_turn: &JOINT4_SMALL_TURN + -1.497 joint5: mechanical: mid_position: &JOINT5_MID_POSITION @@ -139,6 +137,12 @@ common: -0.017 big_store: &JOINT5_BIG_STORE 1.617 + small_ready: &JOINT5_SMALL_READY + -3.128 + small_up_stone: &JOINT5_SMALL_UP_STONE + -0.001 + small_ready_store: &JOINT5_SMALL_READY_STORE + 1.615 joint6: mechanical: @@ -148,8 +152,12 @@ common: -1.569 down_position: &JOINT6_DOWN_POSITION 1.569 - small_ready_second: &JOINT6_SMALL_READY_SECOND - 0.694 + small_ready: &JOINT6_SMALL_READY + -1.607 + small_back: &JOINT6_SMALL_BACK + -1.987 + small_ready_store: &JOINT6_SMALL_READY_STORE + -1.633 bin_get_stone: &JOINT6_BIN_GET_STONE -1.611 big_ready: &JOINT6_BIG_READY @@ -169,6 +177,10 @@ common: 1.62 big_pos: &JOINT7_BIG_POS -0.013 + small_ready: &JOINT7_SMALL_READY + 0.012 + small_ready_store: &JOINT7_SMALL_READY_STORE + 1.722 gimbal: side_pos: &SIDE_POS @@ -187,14 +199,13 @@ common: small_island: &LIFTER_SMALL_ISLAND 0.1 mid_pos: &MID_POS - 0.1 - big_island: &LIFTER_BIG_ISLAND - 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 @@ -284,6 +295,10 @@ steps_list: - step: "gimbal lifter down" gimbal_lifter: target: *BUTTON_POS + - step: "close ex arm" + extend_arm_front: + front: *EX_CLOSE + back: *EX_CLOSE - step: "close gripper" gripper: <<: *CLOSE_GRIPPER @@ -304,6 +319,10 @@ steps_list: - step: "gimbal lifter down" gimbal_lifter: target: *MID_POS + - step: "close ex arm" + extend_arm_front: + front: *EX_CLOSE + back: *EX_CLOSE - step: "close gripper" gripper: <<: *CLOSE_GRIPPER @@ -324,6 +343,10 @@ steps_list: - step: "gimbal lifter down" gimbal_lifter: target: *MID_POS + - step: "close ex arm" + extend_arm_front: + front: *EX_CLOSE + back: *EX_CLOSE - step: "close gripper" gripper: <<: *CLOSE_GRIPPER @@ -347,215 +370,284 @@ steps_list: gripper: <<: *CLOSE_GRIPPER - ################### SMALL_ISLAND_FROM_SIDE ################ + ################### SMALL_ISLAND_FROM_UP ################ ########ONE_STONE_ONCE######### - HAS0_ONE_STONE_SMALL_ISLAND: + ONE_STONE_SMALL_ISLAND: - step: "GIMBAL_READY" gimbal: - <<: *SMALL_ISLAND_POS + <<: *FRONT_POS - step: "ORE_ROTATOR_READY" ore_rotator: target: *READY_POS - step: "ORE_LIFTER_READY" ore_lifter: target: *BIN_MID_POS - - step: "SMALL_ARM_READY" + - step: "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 ] + joints: [ *JOINT1_SMALL_READY, *JOINT2_SMALL_READY, *JOINT3_SMALL_READY_MID, *JOINT4_SMALL_READY, *JOINT5_SMALL_READY, *JOINT6_SMALL_READY, *JOINT7_SMALL_READY ] common: <<: *NORMALLY tolerance: <<: *NORMAL_TOLERANCE - - step: "OPEN_GRIPPER" gripper: <<: *OPEN_GRIPPER - HAS0_ONE_STONE_SMALL_ISLAND0: - - step: "SMALL_ARM_READY" + ONE_STONE_SMALL_ISLAND0: + - step: "GET_STONE" 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_SMALL_DOWN, *JOINT2_SMALL_READY, *JOINT3_SMALL_READY_MID, *JOINT4_SMALL_READY, *JOINT5_SMALL_READY, *JOINT6_SMALL_READY, *JOINT7_SMALL_READY ] common: <<: *NORMALLY tolerance: <<: *NORMAL_TOLERANCE - - step: "SMALL_GET_STONE" + - step: "JOINT1_UP" arm: - joints: [ *JOINT1_SMALL_READY, *JOINT2_BACK_POSITION, *JOINT3_SMALL_GET_STONE, *JOINT4_R_POSITION, *JOINT5_MID_POSITION, *JOINT6_MID_POSITION, *JOINT7_MID_POSITION ] + joints: [ *JOINT1_SMALL_READY, "KEEP", "KEEP", "KEEP", "KEEP", "KEEP", "KEEP" ] common: <<: *NORMALLY tolerance: <<: *NORMAL_TOLERANCE - - step: "JOINT1_UP AND BACK" + - step: "ARM_UP_STONE" arm: - joints: [ *JOINT1_UP_POS, "KEEP", *JOINT3_SMALL_GET_STONE2, "KEEP", "KEEP", "KEEP", "KEEP"] + joints: [ *JOINT1_SMALL_READY, *JOINT2_SMALL_READY, *JOINT3_SMALL_READY_MID, *JOINT4_SMALL_READY, *JOINT5_SMALL_UP_STONE, *JOINT6_SMALL_READY, *JOINT7_SMALL_READY ] common: <<: *NORMALLY tolerance: <<: *NORMAL_TOLERANCE - - step: "SMALL_ARM_UP_STONE" + - step: "JOINT6_BACK" arm: - joints: [ *JOINT1_UP_POS, *JOINT2_SMALL_ARM_UP, *JOINT3_SMALL_GET_STONE2, *JOINT4_R_POSITION, *JOINT5_MID_POSITION, *JOINT6_UP_POSITION, *JOINT7_MID_POSITION ] + joints: [ "KEEP", "KEEP", "KEEP", "KEEP", "KEEP", *JOINT6_SMALL_BACK, "KEEP" ] common: - <<: *SLOWLY + <<: *NORMALLY tolerance: <<: *NORMAL_TOLERANCE - - step: "READY_TO_STORE_STONE" + - step: "TURN_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 ] + joints: [ *JOINT1_SMALL_READY, *JOINT2_SMALL_READY, *JOINT3_SMALL_TURN, *JOINT4_SMALL_TURN, *JOINT5_SMALL_UP_STONE, *JOINT6_SMALL_READY, *JOINT7_SMALL_READY ] common: <<: *NORMALLY tolerance: - <<: *SMALL_TOLERANCE - - step: "STORE_STONE" + <<: *NORMAL_TOLERANCE + - step: "READY_STORE" 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 ] + joints: [ *JOINT1_SMALL_READY_STORE, *JOINT2_SMALL_READY, *JOINT3_SMALL_TURN, *JOINT4_SMALL_TURN, *JOINT5_SMALL_UP_STONE, *JOINT6_SMALL_READY_STORE, *JOINT7_SMALL_READY_STORE ] common: - <<: *SLOWLY + <<: *NORMALLY tolerance: - <<: *SMALL_TOLERANCE - - step: "JOINT5_TURN" + <<: *NORMAL_TOLERANCE + - step: "STORE" 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 ] + joints: [ *JOINT1_SMALL_READY_STORE, *JOINT2_SMALL_READY, *JOINT3_SMALL_TURN, *JOINT4_SMALL_TURN, *JOINT5_SMALL_READY_STORE, *JOINT6_SMALL_READY_STORE, *JOINT7_SMALL_READY_STORE ] common: <<: *SLOWLY tolerance: - <<: *SMALL_TOLERANCE + <<: *NORMAL_TOLERANCE - step: "CLOSE_GRIPPER" gripper: <<: *CLOSE_GRIPPER - - step: "ARM_RESET" + - step: "MOVE_ARM_OUT" 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 ] + joints: [ *JOINT1_SMALL_READY_STORE, *JOINT2_SMALL_READY, *JOINT3_SMALL_TURN, *JOINT4_R_POSITION, *JOINT5_MID_POSITION, *JOINT6_UP_POSITION, *JOINT7_MID_POSITION ] + common: + <<: *SLOWLY + tolerance: + <<: *NORMAL_TOLERANCE + - step: "ORE_LIFTER_DOWN" + ore_lifter: + target: *BIN_DOWN_POS + - step: "ADD_STONE" + stone_num: + change: "+1" + + #######TWO_STONE_ONCE######### + ######FIRST_STONE######## + TWO_STONE_SMALL_ISLAND: + - step: "GIMBAL_READY" + gimbal: + <<: *FRONT_POS + - step: "ORE_ROTATOR_READY" + ore_rotator: + target: *READY_POS + - step: "ORE_LIFTER_READY" + ore_lifter: + target: *BIN_MID_POS + - step: "ARM_READY" + arm: + joints: [ *JOINT1_SMALL_READY, *JOINT2_SMALL_READY, *JOINT3_SMALL_READY_LEFT, *JOINT4_SMALL_READY, *JOINT5_SMALL_READY, *JOINT6_SMALL_READY, *JOINT7_SMALL_READY ] common: <<: *NORMALLY tolerance: <<: *NORMAL_TOLERANCE - - step: "home" + - step: "OPEN_GRIPPER" + gripper: + <<: *OPEN_GRIPPER + TWO_STONE_SMALL_ISLAND0: + - step: "GET_STONE" arm: - joints: [ *JOINT1_ZERO_STONE_POSITION, *JOINT2_BACK_POSITION, *JOINT3_HOME_POS, *JOINT4_R_POSITION, *JOINT5_MID_POSITION, *JOINT6_UP_POSITION, *JOINT7_MID_POSITION ] + joints: [ *JOINT1_SMALL_DOWN, *JOINT2_SMALL_READY, *JOINT3_SMALL_READY_LEFT, *JOINT4_SMALL_READY, *JOINT5_SMALL_READY, *JOINT6_SMALL_READY, *JOINT7_SMALL_READY ] common: <<: *NORMALLY tolerance: <<: *NORMAL_TOLERANCE + - step: "JOINT1_UP" + arm: + joints: [ *JOINT1_SMALL_READY, "KEEP", "KEEP", "KEEP", "KEEP", "KEEP", "KEEP" ] + common: + <<: *NORMALLY + tolerance: + <<: *NORMAL_TOLERANCE + - step: "ARM_UP_STONE" + arm: + joints: [ *JOINT1_SMALL_READY, *JOINT2_SMALL_READY, *JOINT3_SMALL_READY_LEFT, *JOINT4_SMALL_READY, *JOINT5_SMALL_UP_STONE, *JOINT6_SMALL_READY, *JOINT7_SMALL_READY ] + common: + <<: *NORMALLY + tolerance: + <<: *NORMAL_TOLERANCE + - step: "JOINT6_BACK" + arm: + joints: [ "KEEP", "KEEP", "KEEP", "KEEP", "KEEP", *JOINT6_SMALL_BACK, "KEEP" ] + common: + <<: *NORMALLY + tolerance: + <<: *NORMAL_TOLERANCE + - step: "TURN_STONE" + arm: + joints: [ *JOINT1_SMALL_READY, *JOINT2_SMALL_READY, *JOINT3_SMALL_TURN, *JOINT4_SMALL_TURN, *JOINT5_SMALL_UP_STONE, *JOINT6_SMALL_READY, *JOINT7_SMALL_READY ] + common: + <<: *NORMALLY + tolerance: + <<: *NORMAL_TOLERANCE + - step: "READY_STORE" + arm: + joints: [ *JOINT1_SMALL_READY_STORE, *JOINT2_SMALL_READY, *JOINT3_SMALL_TURN, *JOINT4_SMALL_TURN, *JOINT5_SMALL_UP_STONE, *JOINT6_SMALL_READY_STORE, *JOINT7_SMALL_READY_STORE ] + common: + <<: *NORMALLY + tolerance: + <<: *NORMAL_TOLERANCE + - step: "STORE" + arm: + joints: [ *JOINT1_SMALL_READY_STORE, *JOINT2_SMALL_READY, *JOINT3_SMALL_TURN, *JOINT4_SMALL_TURN, *JOINT5_SMALL_READY_STORE, *JOINT6_SMALL_READY_STORE, *JOINT7_SMALL_READY_STORE ] + common: + <<: *SLOWLY + tolerance: + <<: *NORMAL_TOLERANCE + - step: "CLOSE_GRIPPER" + gripper: + <<: *CLOSE_GRIPPER + - step: "MOVE_ARM_OUT" + arm: + joints: [ *JOINT1_SMALL_READY_STORE, *JOINT2_SMALL_READY, *JOINT3_SMALL_TURN, *JOINT4_R_POSITION, *JOINT5_MID_POSITION, *JOINT6_UP_POSITION, *JOINT7_MID_POSITION ] + common: + <<: *SLOWLY + tolerance: + <<: *NORMAL_TOLERANCE - step: "ORE_LIFTER_DOWN" ore_lifter: target: *BIN_DOWN_POS - - step: "add stone" + - step: "ADD_STONE" stone_num: change: "+1" - - step: "GIMBAL_READY" - gimbal: - <<: *FRONT_POS - HAS1_ONE_STONE_SMALL_ISLAND: + #####SECOND_STONE######## +# TWO_STONE_SMALL_ISLAND00: - step: "GIMBAL_READY" gimbal: - <<: *SMALL_ISLAND_POS + <<: *FRONT_POS - step: "ORE_ROTATOR_READY" ore_rotator: target: *READY_POS - step: "ORE_LIFTER_READY" ore_lifter: target: *BIN_DOWN_POS - - step: "SMALL_ARM_READY" + - step: "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 ] + joints: [ *JOINT1_SMALL_READY, *JOINT2_SMALL_READY, *JOINT3_SMALL_READY_MID, *JOINT4_SMALL_READY, *JOINT5_SMALL_READY, *JOINT6_SMALL_READY, *JOINT7_SMALL_READY ] common: <<: *NORMALLY tolerance: <<: *NORMAL_TOLERANCE - - step: "OPEN_GRIPPER" gripper: <<: *OPEN_GRIPPER - ONE_STONE_SMALL_ISLAND0: - - step: "SMALL_ARM_READY" +# TWO_STONE_SMALL_ISLAND000: + - step: "GET_STONE" 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_SMALL_DOWN, *JOINT2_SMALL_READY, *JOINT3_SMALL_READY_MID, *JOINT4_SMALL_READY, *JOINT5_SMALL_READY, *JOINT6_SMALL_READY, *JOINT7_SMALL_READY ] common: <<: *NORMALLY tolerance: <<: *NORMAL_TOLERANCE - - step: "SMALL_GET_STONE" + - step: "JOINT1_UP" arm: - joints: [ *JOINT1_SMALL_READY, *JOINT2_BACK_POSITION, *JOINT3_SMALL_GET_STONE, *JOINT4_R_POSITION, *JOINT5_MID_POSITION, *JOINT6_MID_POSITION, *JOINT7_MID_POSITION ] + joints: [ *JOINT1_SMALL_READY, "KEEP", "KEEP", "KEEP", "KEEP", "KEEP", "KEEP" ] common: <<: *NORMALLY tolerance: <<: *NORMAL_TOLERANCE - - step: "JOINT1_UP AND BACK" + - step: "ARM_UP_STONE" arm: - joints: [ *JOINT1_UP_POS, "KEEP", *JOINT3_SMALL_GET_STONE2, "KEEP", "KEEP", "KEEP", "KEEP"] + joints: [ *JOINT1_SMALL_READY, *JOINT2_SMALL_READY, *JOINT3_SMALL_READY_MID, *JOINT4_SMALL_READY, *JOINT5_SMALL_UP_STONE, *JOINT6_SMALL_READY, *JOINT7_SMALL_READY ] common: <<: *NORMALLY tolerance: <<: *NORMAL_TOLERANCE - - step: "SMALL_ARM_UP_STONE" + - step: "JOINT6_BACK" arm: - joints: [ *JOINT1_UP_POS, *JOINT2_SMALL_ARM_UP, *JOINT3_SMALL_GET_STONE2, *JOINT4_R_POSITION, *JOINT5_MID_POSITION, *JOINT6_UP_POSITION, *JOINT7_MID_POSITION ] + joints: [ "KEEP", "KEEP", "KEEP", "KEEP", "KEEP", *JOINT6_SMALL_BACK, "KEEP" ] common: - <<: *SLOWLY + <<: *NORMALLY tolerance: <<: *NORMAL_TOLERANCE - - step: "READY_TO_STORE_STONE" + - step: "TURN_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 ] + joints: [ *JOINT1_SMALL_READY, *JOINT2_SMALL_READY, *JOINT3_SMALL_TURN, *JOINT4_SMALL_TURN, *JOINT5_SMALL_UP_STONE, *JOINT6_SMALL_READY, *JOINT7_SMALL_READY ] common: <<: *NORMALLY tolerance: - <<: *SMALL_TOLERANCE - - step: "STORE_STONE" + <<: *NORMAL_TOLERANCE + - step: "READY_STORE" 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 ] + joints: [ *JOINT1_SMALL_READY_STORE, *JOINT2_SMALL_READY, *JOINT3_SMALL_TURN, *JOINT4_SMALL_TURN, *JOINT5_SMALL_UP_STONE, *JOINT6_SMALL_READY_STORE, *JOINT7_SMALL_READY_STORE ] common: - <<: *SLOWLY + <<: *NORMALLY tolerance: - <<: *SMALL_TOLERANCE - - step: "JOINT5_TURN" + <<: *NORMAL_TOLERANCE + - step: "STORE" 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 ] + joints: [ *JOINT1_SMALL_READY_STORE, *JOINT2_SMALL_READY, *JOINT3_SMALL_TURN, *JOINT4_SMALL_TURN, *JOINT5_SMALL_READY_STORE, *JOINT6_SMALL_READY_STORE, *JOINT7_SMALL_READY_STORE ] common: <<: *SLOWLY tolerance: - <<: *SMALL_TOLERANCE + <<: *NORMAL_TOLERANCE - step: "CLOSE_GRIPPER" gripper: <<: *CLOSE_GRIPPER - - step: "ARM_RESET" + - step: "MOVE_ARM_OUT" 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 ] + joints: [ *JOINT1_SMALL_READY_STORE, *JOINT2_SMALL_READY, *JOINT3_SMALL_TURN, *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 + <<: *SLOWLY tolerance: <<: *NORMAL_TOLERANCE - - step: "add stone" + - step: "ADD_STONE" stone_num: change: "+1" + + ######THREE_STONE_ONCE(yi jian san lian)######### + ######FIRST_STONE######## + THREE_STONE_SMALL_ISLAND: - step: "GIMBAL_READY" gimbal: <<: *FRONT_POS - ##########TWO_STONE_ONCE########### - ####FIRST_STONE### - TWO_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_MID_POS - - step: "SMALL_ARM_READY" + - step: "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 ] + joints: [ *JOINT1_SMALL_READY, *JOINT2_SMALL_READY, *JOINT3_SMALL_READY_LEFT, *JOINT4_SMALL_READY, *JOINT5_SMALL_READY, *JOINT6_SMALL_READY, *JOINT7_SMALL_READY ] common: <<: *NORMALLY tolerance: @@ -563,77 +655,87 @@ steps_list: - step: "OPEN_GRIPPER" gripper: <<: *OPEN_GRIPPER - TWO_STONE_SMALL_ISLAND0: - - step: "SMALL_ARM_READY" + THREE_STONE_SMALL_ISLAND0: + - step: "GET_STONE" 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_SMALL_DOWN, *JOINT2_SMALL_READY, *JOINT3_SMALL_READY_LEFT, *JOINT4_SMALL_READY, *JOINT5_SMALL_READY, *JOINT6_SMALL_READY, *JOINT7_SMALL_READY ] common: <<: *NORMALLY tolerance: <<: *NORMAL_TOLERANCE - - step: "SMALL_GET_STONE" + - step: "JOINT1_UP" arm: - joints: [ *JOINT1_SMALL_READY, *JOINT2_BACK_POSITION, *JOINT3_SMALL_GET_STONE, *JOINT4_R_POSITION, *JOINT5_MID_POSITION, *JOINT6_MID_POSITION, *JOINT7_MID_POSITION ] + joints: [ *JOINT1_SMALL_READY, "KEEP", "KEEP", "KEEP", "KEEP", "KEEP", "KEEP" ] common: <<: *NORMALLY tolerance: <<: *NORMAL_TOLERANCE - - step: "JOINT1_UP" + - step: "ARM_UP_STONE" arm: - joints: [ *JOINT1_UP_POS, "KEEP", "KEEP", "KEEP", "KEEP", "KEEP", "KEEP"] + joints: [ *JOINT1_SMALL_READY, *JOINT2_SMALL_READY, *JOINT3_SMALL_READY_LEFT, *JOINT4_SMALL_READY, *JOINT5_SMALL_UP_STONE, *JOINT6_SMALL_READY, *JOINT7_SMALL_READY ] common: <<: *NORMALLY tolerance: <<: *NORMAL_TOLERANCE - - step: "SMALL_ARM_UP_STONE" + - step: "JOINT6_BACK" 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: [ "KEEP", "KEEP", "KEEP", "KEEP", "KEEP", *JOINT6_SMALL_BACK, "KEEP" ] common: - <<: *SLOWLY + <<: *NORMALLY tolerance: <<: *NORMAL_TOLERANCE - - step: "READY_TO_STORE_STONE" + - step: "TURN_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 ] + joints: [ *JOINT1_SMALL_READY, *JOINT2_SMALL_READY, *JOINT3_SMALL_TURN, *JOINT4_SMALL_TURN, *JOINT5_SMALL_UP_STONE, *JOINT6_SMALL_READY, *JOINT7_SMALL_READY ] common: <<: *NORMALLY tolerance: - <<: *SMALL_TOLERANCE - - step: "STORE_STONE" + <<: *NORMAL_TOLERANCE + - step: "READY_STORE" 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 ] + joints: [ *JOINT1_SMALL_READY_STORE, *JOINT2_SMALL_READY, *JOINT3_SMALL_TURN, *JOINT4_SMALL_TURN, *JOINT5_SMALL_UP_STONE, *JOINT6_SMALL_READY_STORE, *JOINT7_SMALL_READY_STORE ] common: - <<: *SLOWLY + <<: *NORMALLY tolerance: - <<: *SMALL_TOLERANCE - - step: "JOINT5_TURN" + <<: *NORMAL_TOLERANCE + - step: "STORE" 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 ] + joints: [ *JOINT1_SMALL_READY_STORE, *JOINT2_SMALL_READY, *JOINT3_SMALL_TURN, *JOINT4_SMALL_TURN, *JOINT5_SMALL_READY_STORE, *JOINT6_SMALL_READY_STORE, *JOINT7_SMALL_READY_STORE ] common: <<: *SLOWLY tolerance: - <<: *SMALL_TOLERANCE + <<: *NORMAL_TOLERANCE - step: "CLOSE_GRIPPER" gripper: <<: *CLOSE_GRIPPER - - step: "ARM_RESET" + - step: "MOVE_ARM_OUT" 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 ] + joints: [ *JOINT1_SMALL_READY_STORE, *JOINT2_SMALL_READY, *JOINT3_SMALL_TURN, *JOINT4_R_POSITION, *JOINT5_MID_POSITION, *JOINT6_UP_POSITION, *JOINT7_MID_POSITION ] common: - <<: *NORMALLY + <<: *SLOWLY tolerance: <<: *NORMAL_TOLERANCE - step: "ORE_LIFTER_DOWN" ore_lifter: target: *BIN_DOWN_POS - - step: "add stone" + - step: "ADD_STONE" stone_num: change: "+1" - ###SECOND_STONE### - TWO_STONE_SMALL_ISLAND00: - - step: "SMALL_ARM_READY_TWO" + + #####SECOND_STONE######## +# THREE_STONE_SMALL_ISLAND00: + - step: "GIMBAL_READY" + gimbal: + <<: *FRONT_POS + - step: "ORE_ROTATOR_READY" + ore_rotator: + target: *READY_POS + - step: "ORE_LIFTER_READY" + ore_lifter: + target: *BIN_DOWN_POS + - step: "ARM_READY" arm: - 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 ] + joints: [ *JOINT1_SMALL_READY, *JOINT2_SMALL_READY, *JOINT3_SMALL_READY_MID, *JOINT4_SMALL_READY, *JOINT5_SMALL_READY, *JOINT6_SMALL_READY, *JOINT7_SMALL_READY ] common: <<: *NORMALLY tolerance: @@ -641,70 +743,155 @@ steps_list: - step: "OPEN_GRIPPER" gripper: <<: *OPEN_GRIPPER - TWO_STONE_SMALL_ISLAND000: - - step: "SMALL_GET_STONE_TWO" +# THREE_STONE_SMALL_ISLAND000: + - step: "GET_STONE" arm: - 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 ] + joints: [ *JOINT1_SMALL_DOWN, *JOINT2_SMALL_READY, *JOINT3_SMALL_READY_MID, *JOINT4_SMALL_READY, *JOINT5_SMALL_READY, *JOINT6_SMALL_READY, *JOINT7_SMALL_READY ] common: <<: *NORMALLY tolerance: - <<: *SMALL_TOLERANCE + <<: *NORMAL_TOLERANCE - step: "JOINT1_UP" arm: - joints: [ *JOINT1_UP_POS, "KEEP", "KEEP", "KEEP", "KEEP", "KEEP", "KEEP"] + joints: [ *JOINT1_SMALL_READY, "KEEP", "KEEP", "KEEP", "KEEP", "KEEP", "KEEP" ] common: <<: *NORMALLY tolerance: <<: *NORMAL_TOLERANCE - - step: "SMALL_ARM_LIFT_STONE" + - step: "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_SMALL_READY, *JOINT2_SMALL_READY, *JOINT3_SMALL_READY_MID, *JOINT4_SMALL_READY, *JOINT5_SMALL_UP_STONE, *JOINT6_SMALL_READY, *JOINT7_SMALL_READY ] common: <<: *NORMALLY tolerance: <<: *NORMAL_TOLERANCE - - step: "READY_TO_STORE_STONE" + - step: "JOINT6_BACK" 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 ] + joints: [ "KEEP", "KEEP", "KEEP", "KEEP", "KEEP", *JOINT6_SMALL_BACK, "KEEP" ] common: <<: *NORMALLY tolerance: - <<: *SMALL_TOLERANCE - - step: "STORE_STONE" + <<: *NORMAL_TOLERANCE + - step: "TURN_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 ] + joints: [ *JOINT1_SMALL_READY, *JOINT2_SMALL_READY, *JOINT3_SMALL_TURN, *JOINT4_SMALL_TURN, *JOINT5_SMALL_UP_STONE, *JOINT6_SMALL_READY, *JOINT7_SMALL_READY ] common: <<: *NORMALLY tolerance: - <<: *SMALL_TOLERANCE - - step: "JOINT5" + <<: *NORMAL_TOLERANCE + - step: "READY_STORE" 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 ] + joints: [ *JOINT1_SMALL_READY_STORE, *JOINT2_SMALL_READY, *JOINT3_SMALL_TURN, *JOINT4_SMALL_TURN, *JOINT5_SMALL_UP_STONE, *JOINT6_SMALL_READY_STORE, *JOINT7_SMALL_READY_STORE ] common: <<: *NORMALLY tolerance: - <<: *SMALL_TOLERANCE + <<: *NORMAL_TOLERANCE + - step: "STORE" + arm: + joints: [ *JOINT1_SMALL_READY_STORE, *JOINT2_SMALL_READY, *JOINT3_SMALL_TURN, *JOINT4_SMALL_TURN, *JOINT5_SMALL_READY_STORE, *JOINT6_SMALL_READY_STORE, *JOINT7_SMALL_READY_STORE ] + common: + <<: *SLOWLY + tolerance: + <<: *NORMAL_TOLERANCE - step: "CLOSE_GRIPPER" gripper: <<: *CLOSE_GRIPPER - - step: "ARM_RESET" + - step: "MOVE_ARM_OUT" + arm: + joints: [ *JOINT1_SMALL_READY_STORE, *JOINT2_SMALL_READY, *JOINT3_SMALL_TURN, *JOINT4_R_POSITION, *JOINT5_MID_POSITION, *JOINT6_UP_POSITION, *JOINT7_MID_POSITION ] + common: + <<: *SLOWLY + tolerance: + <<: *NORMAL_TOLERANCE + - step: "ADD_STONE" + stone_num: + change: "+1" + #######THIRD_STONE######### +# THREE_STONE_SMALL_ISLAND0000: + - step: "GIMBAL_READY" + gimbal: + <<: *FRONT_POS + - step: "ORE_ROTATOR_READY" + ore_rotator: + target: *READY_POS + - step: "ORE_LIFTER_READY" + ore_lifter: + target: *BIN_DOWN_POS + - step: "ARM_READY" 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 ] + joints: [ *JOINT1_SMALL_READY, *JOINT2_SMALL_READY, *JOINT3_SMALL_READY_RIGHT, *JOINT4_SMALL_READY, *JOINT5_SMALL_READY, *JOINT6_SMALL_READY, *JOINT7_SMALL_READY ] common: <<: *NORMALLY tolerance: <<: *NORMAL_TOLERANCE - - step: "home" + - step: "OPEN_GRIPPER" + gripper: + <<: *OPEN_GRIPPER +# THREE_STONE_SMALL_ISLAND00000: + - step: "GET_STONE" arm: - joints: [ *JOINT1_ZERO_STONE_POSITION, *JOINT2_BACK_POSITION, *JOINT3_HOME_POS, *JOINT4_R_POSITION, *JOINT5_MID_POSITION, *JOINT6_UP_POSITION, *JOINT7_MID_POSITION ] + joints: [ *JOINT1_SMALL_DOWN, *JOINT2_SMALL_READY, *JOINT3_SMALL_READY_RIGHT, *JOINT4_SMALL_READY, *JOINT5_SMALL_READY, *JOINT6_SMALL_READY, *JOINT7_SMALL_READY ] common: <<: *NORMALLY tolerance: <<: *NORMAL_TOLERANCE - - step: "add stone" + - step: "JOINT1_UP" + arm: + joints: [ *JOINT1_SMALL_READY, "KEEP", "KEEP", "KEEP", "KEEP", "KEEP", "KEEP" ] + common: + <<: *NORMALLY + tolerance: + <<: *NORMAL_TOLERANCE + - step: "ARM_UP_STONE" + arm: + joints: [ *JOINT1_SMALL_READY, *JOINT2_SMALL_READY, *JOINT3_SMALL_READY_RIGHT, *JOINT4_SMALL_READY, *JOINT5_SMALL_UP_STONE, *JOINT6_SMALL_READY, *JOINT7_SMALL_READY ] + common: + <<: *NORMALLY + tolerance: + <<: *NORMAL_TOLERANCE + - step: "JOINT6_BACK" + arm: + joints: [ "KEEP", "KEEP", "KEEP", "KEEP", "KEEP", *JOINT6_SMALL_BACK, "KEEP" ] + common: + <<: *NORMALLY + tolerance: + <<: *NORMAL_TOLERANCE + - step: "TURN_STONE" + arm: + joints: [ *JOINT1_SMALL_READY, *JOINT2_SMALL_READY, *JOINT3_SMALL_TURN, *JOINT4_SMALL_TURN, *JOINT5_SMALL_UP_STONE, *JOINT6_SMALL_READY, *JOINT7_SMALL_READY ] + common: + <<: *NORMALLY + tolerance: + <<: *NORMAL_TOLERANCE + - step: "READY_STORE(THIRD_STONE)" + arm: + joints: [ *JOINT1_SMALL_READY_STORE_THIRD, *JOINT2_SMALL_READY, *JOINT3_SMALL_TURN, *JOINT4_SMALL_TURN, *JOINT5_SMALL_UP_STONE, *JOINT6_SMALL_READY_STORE, *JOINT7_SMALL_READY_STORE ] + common: + <<: *NORMALLY + tolerance: + <<: *NORMAL_TOLERANCE + - step: "STORE" + arm: + joints: [ *JOINT1_SMALL_READY_STORE_THIRD, *JOINT2_SMALL_READY, *JOINT3_SMALL_TURN, *JOINT4_SMALL_TURN, *JOINT5_SMALL_READY_STORE, *JOINT6_SMALL_READY_STORE, *JOINT7_SMALL_READY_STORE ] + common: + <<: *SLOWLY + tolerance: + <<: *NORMAL_TOLERANCE +# - step: "CLOSE_GRIPPER" +# gripper: +# <<: *CLOSE_GRIPPER +# - step: "MOVE_ARM_OUT" +# arm: +# joints: [ *JOINT1_SMALL_READY_STORE_THIRD, *JOINT2_SMALL_READY, *JOINT3_SMALL_TURN, *JOINT4_R_POSITION, *JOINT5_MID_POSITION, *JOINT6_UP_POSITION, *JOINT7_MID_POSITION ] +# common: +# <<: *SLOWLY +# tolerance: +# <<: *NORMAL_TOLERANCE + - step: "ADD_STONE" stone_num: change: "+1" + ######GET STONE FROM BIN######### GET_DOWN_STONE_BIN: - step: "GIMBAL_READY" gimbal: @@ -818,6 +1005,10 @@ steps_list: - 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 ] @@ -917,10 +1108,16 @@ steps_list: 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 ] @@ -1021,6 +1218,10 @@ steps_list: - 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 ] @@ -1040,6 +1241,7 @@ steps_list: - step: "OPEN_GRIPPER" gripper: <<: *OPEN_GRIPPER + HAS0_SIDE_BIG_ISLAND0: - step: "GET_MID_STONE" arm: @@ -1124,6 +1326,9 @@ steps_list: - 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 ] @@ -1250,6 +1455,9 @@ steps_list: 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 @@ -1276,7 +1484,7 @@ steps_list: arm: frame: exchanger is_refer_planning_frame: true - xyz: [ -0.2, 0., 0. ] + xyz: [ -0.3, 0., 0. ] rpy: [ 0., 3.1415926, 0. ] tolerance_position: 0.01 tolerance_orientation: 0.001 From f32d12f95404fd31b599b02336ea8debd81810b0 Mon Sep 17 00:00:00 2001 From: cc0h <7921166012@qq.com> Date: Wed, 24 Apr 2024 01:05:54 +0800 Subject: [PATCH 10/80] Record. --- engineer_middleware/config/steps_list.yaml | 410 +++++---------------- 1 file changed, 90 insertions(+), 320 deletions(-) diff --git a/engineer_middleware/config/steps_list.yaml b/engineer_middleware/config/steps_list.yaml index 96a1678..a84a101 100644 --- a/engineer_middleware/config/steps_list.yaml +++ b/engineer_middleware/config/steps_list.yaml @@ -17,7 +17,7 @@ common: small_tolerance: &SMALL_TOLERANCE 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 ] + tolerance_joints: [ 0.01, 0.010, 0.015, 0.3, 0.2, 0.15, 0.07 ] bigger_tolerance: &BIGGER_TOLERANCE tolerance_joints: [ 0.005, 0.010, 0.01, 0.3, 0.2, 0.2 ] max_tolerance: &MAX_TOLERANCE @@ -28,11 +28,11 @@ common: down_position: &JOINT1_DOWN_POSITION 0.02 small_ready: &JOINT1_SMALL_READY - 0.475 + 0.480 small_down: &JOINT1_SMALL_DOWN 0.31 small_ready_store: &JOINT1_SMALL_READY_STORE - 0.249 + 0.254 small_ready_store_third: &JOINT1_SMALL_READY_STORE_THIRD 0.387 up_pos: &JOINT1_UP_POS @@ -46,7 +46,7 @@ common: big_up_pos: &JOINT1_BIG_UP_POS 0.320 big_ready_store: &JOINT1_BIG_READY_STORE - 0.155 + 0.233 home: zero_stone_position: &JOINT1_ZERO_STONE_POSITION 0.01 @@ -68,30 +68,30 @@ common: exchange_position: &JOINT2_EXCHANGE_POSITION 0.07 small_ready: &JOINT2_SMALL_READY - 0.025 + 0.034 bin_get_stone: &JOINT2_BIN_GET_STONE 0.087 back_bin_position: &JOINT2_BACK_BIN_POSITION 0.001 big_ready: &JOINT2_BIG_READY - 0.009 + 0.03 big_arm_up: &JOINT2_BIG_ARM_UP 0.142 big_ready_store: &JOINT2_BIG_READY_STORE - 0.020 + 0.03 joint3: mechanical: left_position: &JOINT3_L_POSITION - 0.28 + 0.308 mid_position: &JOINT3_MID_POSITION 0.04 right_position: &JOINT3_R_POSITION - -0.275 + -0.278 home: &JOINT3_HOME_POS 0.08 small_ready_mid: &JOINT3_SMALL_READY_MID - 0.037 + 0.032 small_ready_left: &JOINT3_SMALL_READY_LEFT 0.308 small_ready_right: &JOINT3_SMALL_READY_RIGHT @@ -103,13 +103,13 @@ common: big_ready: &JOINT3_BIG_READY 0.071 get_big_mid: &JOINT3_GET_BIG_MID - -0.281 + -0.292 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.291 + 0.267 joint4: mechanical: @@ -118,11 +118,13 @@ common: mid_position: &JOINT4_MID_POSITION 0.001 right_position: &JOINT4_R_POSITION - -1.495 + -1.52 small_ready: &JOINT4_SMALL_READY 0.054 small_turn: &JOINT4_SMALL_TURN -1.497 + big_store: &JOINT4_BIG_STORE + -1.525 joint5: mechanical: mid_position: &JOINT5_MID_POSITION @@ -135,13 +137,15 @@ common: 1.679 big_ready: &JOINT5_BIG_READY -0.017 + big_ready_store: &JOINT5_BIG_READY_STORE + -0.001 big_store: &JOINT5_BIG_STORE - 1.617 + 1.49 small_ready: &JOINT5_SMALL_READY -3.128 small_up_stone: &JOINT5_SMALL_UP_STONE -0.001 - small_ready_store: &JOINT5_SMALL_READY_STORE + small_store: &JOINT5_SMALL_STORE 1.615 joint6: @@ -166,6 +170,8 @@ common: -0.119 big_arm_up: &JOINT6_BIG_ARM_UP -1.519 + big_ready_store: &JOINT6_BIG_READY_STORE + -1.561 joint7: mechanical: @@ -177,6 +183,8 @@ common: 1.62 big_pos: &JOINT7_BIG_POS -0.013 + big_ready_store: &JOINT7_BIG_READY_STORE + -1.522 small_ready: &JOINT7_SMALL_READY 0.012 small_ready_store: &JOINT7_SMALL_READY_STORE @@ -195,17 +203,17 @@ common: gimbal_lifter: button_pos: &BUTTON_POS - 0.07 + 0.05 small_island: &LIFTER_SMALL_ISLAND - 0.1 + 0.05 mid_pos: &MID_POS 0.14 big_island: &LIFTER_BIG_ISLAND 0.260 two_stone_pos: &TWO_STONE_POS - 0.163 + 0.05 tallest_pos: &TALLEST_POS - 0.394 + 0.28 gripper: open_gripper: &OPEN_GRIPPER @@ -216,15 +224,15 @@ common: state: false arm: - home: &HOME_1 - joints: [ "VARIABLE", *JOINT2_BACK_POSITION, *JOINT3_HOME_POS, *JOINT4_R_POSITION, *JOINT5_MID_POSITION, *JOINT6_UP_POSITION, *JOINT7_MID_POSITION ] + home: &HOME + 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 - chassis_move: + chassis: chassis_left_315: &CHASSIS_LEFT_15 frame: base_link position: [ 0., 15. ] @@ -288,7 +296,7 @@ steps_list: ############ MID ############# ############################ HOME ########################## - HOME_ZERO_STONE: + HOME: - step: "gimbal look front" gimbal: <<: *FRONT_POS @@ -296,55 +304,7 @@ steps_list: gimbal_lifter: target: *BUTTON_POS - step: "close ex arm" - extend_arm_front: - 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 - variable: [ *JOINT1_ZERO_STONE_POSITION, 0, 0, 0, 0, 0 ] - HOME_ONE_STONE: - - step: "gimbal look front" - gimbal: - <<: *FRONT_POS - - step: "gimbal lifter down" - gimbal_lifter: - target: *MID_POS - - step: "close ex arm" - extend_arm_front: - 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_ZERO_STONE_POSITION, 0, 0, 0, 0, 0 ] - HOME_TWO_STONE: - - step: "gimbal look front" - gimbal: - <<: *FRONT_POS - - step: "gimbal lifter down" - gimbal_lifter: - target: *MID_POS - - step: "close ex arm" - extend_arm_front: + extend_arm: front: *EX_CLOSE back: *EX_CLOSE - step: "close gripper" @@ -356,10 +316,9 @@ steps_list: - step: "ORE_LIFTER_UP" ore_lifter: target: *BIN_DOWN_POS - - step: "home with two stone" + - step: "home" arm: - <<: *HOME_1 - variable: [ *JOINT1_ZERO_STONE_POSITION, 0, 0, 0, 0, 0 ] + <<: *HOME OPEN_GRIPPER: - step: "open gripper" @@ -437,11 +396,12 @@ steps_list: <<: *NORMAL_TOLERANCE - step: "STORE" arm: - joints: [ *JOINT1_SMALL_READY_STORE, *JOINT2_SMALL_READY, *JOINT3_SMALL_TURN, *JOINT4_SMALL_TURN, *JOINT5_SMALL_READY_STORE, *JOINT6_SMALL_READY_STORE, *JOINT7_SMALL_READY_STORE ] + joints: [ *JOINT1_SMALL_READY_STORE, *JOINT2_SMALL_READY, *JOINT3_SMALL_TURN, *JOINT4_SMALL_TURN, *JOINT5_SMALL_STORE, *JOINT6_SMALL_READY_STORE, *JOINT7_SMALL_READY_STORE ] common: <<: *SLOWLY tolerance: <<: *NORMAL_TOLERANCE +# ONE_STONE_SMALL_ISLAND00: - step: "CLOSE_GRIPPER" gripper: <<: *CLOSE_GRIPPER @@ -452,14 +412,17 @@ steps_list: <<: *SLOWLY tolerance: <<: *NORMAL_TOLERANCE + - step: "home" + arm: + <<: *HOME - step: "ORE_LIFTER_DOWN" ore_lifter: target: *BIN_DOWN_POS - step: "ADD_STONE" stone_num: - change: "+1" + change: "SILVER" - #######TWO_STONE_ONCE######### + ########################## TWO_STONE_ONCE ######################## ######FIRST_STONE######## TWO_STONE_SMALL_ISLAND: - step: "GIMBAL_READY" @@ -481,6 +444,9 @@ steps_list: - step: "OPEN_GRIPPER" gripper: <<: *OPEN_GRIPPER + - step: "gimbal lifter ready" + gimbal_lifter: + target: *BUTTON_POS TWO_STONE_SMALL_ISLAND0: - step: "GET_STONE" arm: @@ -526,7 +492,7 @@ steps_list: <<: *NORMAL_TOLERANCE - step: "STORE" arm: - joints: [ *JOINT1_SMALL_READY_STORE, *JOINT2_SMALL_READY, *JOINT3_SMALL_TURN, *JOINT4_SMALL_TURN, *JOINT5_SMALL_READY_STORE, *JOINT6_SMALL_READY_STORE, *JOINT7_SMALL_READY_STORE ] + joints: [ *JOINT1_SMALL_READY_STORE, *JOINT2_SMALL_READY, *JOINT3_SMALL_TURN, *JOINT4_SMALL_TURN, *JOINT5_SMALL_STORE, *JOINT6_SMALL_READY_STORE, *JOINT7_SMALL_READY_STORE ] common: <<: *SLOWLY tolerance: @@ -546,7 +512,7 @@ steps_list: target: *BIN_DOWN_POS - step: "ADD_STONE" stone_num: - change: "+1" + change: "SILVER" #####SECOND_STONE######## # TWO_STONE_SMALL_ISLAND00: @@ -614,7 +580,7 @@ steps_list: <<: *NORMAL_TOLERANCE - step: "STORE" arm: - joints: [ *JOINT1_SMALL_READY_STORE, *JOINT2_SMALL_READY, *JOINT3_SMALL_TURN, *JOINT4_SMALL_TURN, *JOINT5_SMALL_READY_STORE, *JOINT6_SMALL_READY_STORE, *JOINT7_SMALL_READY_STORE ] + joints: [ *JOINT1_SMALL_READY_STORE, *JOINT2_SMALL_READY, *JOINT3_SMALL_TURN, *JOINT4_SMALL_TURN, *JOINT5_SMALL_STORE, *JOINT6_SMALL_READY_STORE, *JOINT7_SMALL_READY_STORE ] common: <<: *SLOWLY tolerance: @@ -629,9 +595,12 @@ steps_list: <<: *SLOWLY tolerance: <<: *NORMAL_TOLERANCE + - step: "home" + arm: + <<: *HOME - step: "ADD_STONE" stone_num: - change: "+1" + change: "SILVER" ######THREE_STONE_ONCE(yi jian san lian)######### ######FIRST_STONE######## @@ -700,7 +669,7 @@ steps_list: <<: *NORMAL_TOLERANCE - step: "STORE" arm: - joints: [ *JOINT1_SMALL_READY_STORE, *JOINT2_SMALL_READY, *JOINT3_SMALL_TURN, *JOINT4_SMALL_TURN, *JOINT5_SMALL_READY_STORE, *JOINT6_SMALL_READY_STORE, *JOINT7_SMALL_READY_STORE ] + joints: [ *JOINT1_SMALL_READY_STORE, *JOINT2_SMALL_READY, *JOINT3_SMALL_TURN, *JOINT4_SMALL_TURN, *JOINT5_SMALL_STORE, *JOINT6_SMALL_READY_STORE, *JOINT7_SMALL_READY_STORE ] common: <<: *SLOWLY tolerance: @@ -720,7 +689,7 @@ steps_list: target: *BIN_DOWN_POS - step: "ADD_STONE" stone_num: - change: "+1" + change: "SILVER" #####SECOND_STONE######## # THREE_STONE_SMALL_ISLAND00: @@ -788,7 +757,7 @@ steps_list: <<: *NORMAL_TOLERANCE - step: "STORE" arm: - joints: [ *JOINT1_SMALL_READY_STORE, *JOINT2_SMALL_READY, *JOINT3_SMALL_TURN, *JOINT4_SMALL_TURN, *JOINT5_SMALL_READY_STORE, *JOINT6_SMALL_READY_STORE, *JOINT7_SMALL_READY_STORE ] + joints: [ *JOINT1_SMALL_READY_STORE, *JOINT2_SMALL_READY, *JOINT3_SMALL_TURN, *JOINT4_SMALL_TURN, *JOINT5_SMALL_STORE, *JOINT6_SMALL_READY_STORE, *JOINT7_SMALL_READY_STORE ] common: <<: *SLOWLY tolerance: @@ -805,7 +774,7 @@ steps_list: <<: *NORMAL_TOLERANCE - step: "ADD_STONE" stone_num: - change: "+1" + change: "SILVER" #######THIRD_STONE######### # THREE_STONE_SMALL_ISLAND0000: - step: "GIMBAL_READY" @@ -872,7 +841,7 @@ steps_list: <<: *NORMAL_TOLERANCE - step: "STORE" arm: - joints: [ *JOINT1_SMALL_READY_STORE_THIRD, *JOINT2_SMALL_READY, *JOINT3_SMALL_TURN, *JOINT4_SMALL_TURN, *JOINT5_SMALL_READY_STORE, *JOINT6_SMALL_READY_STORE, *JOINT7_SMALL_READY_STORE ] + joints: [ *JOINT1_SMALL_READY_STORE_THIRD, *JOINT2_SMALL_READY, *JOINT3_SMALL_TURN, *JOINT4_SMALL_TURN, *JOINT5_SMALL_STORE, *JOINT6_SMALL_READY_STORE, *JOINT7_SMALL_READY_STORE ] common: <<: *SLOWLY tolerance: @@ -889,7 +858,7 @@ steps_list: # <<: *NORMAL_TOLERANCE - step: "ADD_STONE" stone_num: - change: "+1" + change: "SILVER" ######GET STONE FROM BIN######### GET_DOWN_STONE_BIN: @@ -1001,7 +970,7 @@ steps_list: #################BIG_ISLAND############# ######MID_STONE####### - HAS0_MID_BIG_ISLAND: + MID_BIG_ISLAND: - step: "gimbal ready" gimbal: <<: *SMALL_ISLAND_POS @@ -1028,7 +997,7 @@ steps_list: - step: "OPEN_GRIPPER" gripper: <<: *OPEN_GRIPPER - HAS0_MID_BIG_ISLAND0: + 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 ] @@ -1062,25 +1031,26 @@ steps_list: <<: *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 ] + joints: [ *JOINT1_BIG_UP_POS, *JOINT2_BIG_ARM_UP, *JOINT3_BIG_ARM_UP, *JOINT4_BIG_STORE, *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 ] + joints: [ *JOINT1_BIG_READY_STORE, *JOINT2_BIG_READY_STORE, *JOINT3_BIG_ARM_UP, *JOINT4_BIG_STORE, *JOINT5_BIG_READY_STORE, *JOINT6_BIG_READY_STORE, *JOINT7_BIG_READY_STORE ] 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 ] + joints: [ *JOINT1_BIG_READY_STORE, *JOINT2_BIG_READY_STORE, *JOINT3_BIG_ARM_UP, *JOINT4_BIG_STORE, *JOINT5_BIG_STORE, *JOINT6_BIG_READY_STORE, *JOINT7_BIG_READY_STORE ] common: <<: *SLOWLY tolerance: <<: *NORMAL_TOLERANCE +# MID_BIG_ISLAND00: - step: "CLOSE_GRIPPER" gripper: <<: *CLOSE_GRIPPER @@ -1093,128 +1063,19 @@ steps_list: <<: *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 + <<: *HOME - step: "ORE_LIFTER_DOWN" ore_lifter: target: *BIN_DOWN_POS - step: "add stone" stone_num: - change: "+1" + change: "GOLD" - 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 - - 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: + SIDE_BIG_ISLAND: - step: "gimbal ready" gimbal: <<: *SMALL_ISLAND_POS @@ -1242,7 +1103,7 @@ steps_list: gripper: <<: *OPEN_GRIPPER - HAS0_SIDE_BIG_ISLAND0: + 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 ] @@ -1276,21 +1137,21 @@ steps_list: <<: *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 ] + joints: [ *JOINT1_BIG_UP_POS, *JOINT2_BIG_ARM_UP, *JOINT3_BIG_ARM_UP, *JOINT4_BIG_STORE, *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 ] + joints: [ *JOINT1_BIG_READY_STORE, *JOINT2_BIG_READY_STORE, *JOINT3_BIG_ARM_UP, *JOINT4_BIG_STORE, *JOINT5_BIG_READY, *JOINT6_BIG_READY_STORE, *JOINT7_BIG_READY_STORE ] 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 ] + joints: [ *JOINT1_BIG_READY_STORE, *JOINT2_BIG_READY_STORE, *JOINT3_BIG_ARM_UP, *JOINT4_BIG_STORE, *JOINT5_BIG_STORE, *JOINT6_BIG_READY_STORE, *JOINT7_BIG_READY_STORE ] common: <<: *SLOWLY tolerance: @@ -1307,120 +1168,13 @@ steps_list: <<: *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 + <<: *HOME - 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" + change: "GOLD" - step: "gimbal front" gimbal: <<: *FRONT_POS @@ -1450,7 +1204,7 @@ steps_list: arm: frame: exchanger is_refer_planning_frame: true - xyz: [ -0.2, 0., 0. ] + xyz: [ -0.3, 0.2, -0.2 ] rpy: [ 0., -1.5707963, 0. ] drift_dimensions: [ true, true, true, true, true, true ] tolerance_position: 0.01 @@ -1478,7 +1232,21 @@ steps_list: 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: @@ -1486,6 +1254,7 @@ steps_list: 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 @@ -1495,6 +1264,7 @@ steps_list: common: <<: *NORMALLY + GIMBAL_DOWN: - step: "gimbal lowest" gimbal_lifter: From 3ed5c5a8e5b8fd9c0a7a3bb542936eb4d8d10f02 Mon Sep 17 00:00:00 2001 From: cc0h <7921166012@qq.com> Date: Fri, 26 Apr 2024 01:54:19 +0800 Subject: [PATCH 11/80] Change servo command frame. --- engineer_arm_config/config/servo.yaml | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/engineer_arm_config/config/servo.yaml b/engineer_arm_config/config/servo.yaml index a987de8..298263e 100644 --- a/engineer_arm_config/config/servo.yaml +++ b/engineer_arm_config/config/servo.yaml @@ -32,8 +32,8 @@ move_group_name: engineer_arm # Often 'manipulator' or 'arm' planning_frame: base_link # The MoveIt planning frame. Often 'base_link' or 'world' ## Other frames -ee_frame_name: tools_link # The name of the end effector link, used to return the EE pose -robot_link_command_frame: tools_link # commands must be given in the frame of a robot link. Usually either the base or end effector +ee_frame_name: link7 # The name of the end effector link, used to return the EE pose +robot_link_command_frame: link7 # commands must be given in the frame of a robot link. Usually either the base or end effector ## Stopping behaviour incoming_command_timeout: 0.1 # Stop servoing if X seconds elapse without a new command From 9f492fcd9ec141f1be1a9ade818cb0e04e518010 Mon Sep 17 00:00:00 2001 From: cc0h <7921166012@qq.com> Date: Fri, 26 Apr 2024 01:55:15 +0800 Subject: [PATCH 12/80] update step lists. --- engineer_middleware/config/steps_list.yaml | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/engineer_middleware/config/steps_list.yaml b/engineer_middleware/config/steps_list.yaml index a84a101..ff0f46b 100644 --- a/engineer_middleware/config/steps_list.yaml +++ b/engineer_middleware/config/steps_list.yaml @@ -103,7 +103,7 @@ common: big_ready: &JOINT3_BIG_READY 0.071 get_big_mid: &JOINT3_GET_BIG_MID - -0.292 + -0.295 get_big_side: &JOINT3_GET_BIG_SIDE -0.199 big_pull_out: &JOINT3_BIG_PULL_OUT @@ -118,7 +118,7 @@ common: mid_position: &JOINT4_MID_POSITION 0.001 right_position: &JOINT4_R_POSITION - -1.52 + -1.554 small_ready: &JOINT4_SMALL_READY 0.054 small_turn: &JOINT4_SMALL_TURN @@ -209,7 +209,7 @@ common: mid_pos: &MID_POS 0.14 big_island: &LIFTER_BIG_ISLAND - 0.260 + 0.230 two_stone_pos: &TWO_STONE_POS 0.05 tallest_pos: &TALLEST_POS From 25fcf7a7fc945e1c11298563bf7d112bd1c7853c Mon Sep 17 00:00:00 2001 From: cc0h <7921166012@qq.com> Date: Sat, 27 Apr 2024 23:26:44 +0800 Subject: [PATCH 13/80] Add moveit_exchange.h. --- engineer_middleware/CMakeLists.txt | 3 +- .../engineer_middleware/moveit_exchange.h | 104 ++++++++++++++++++ 2 files changed, 106 insertions(+), 1 deletion(-) create mode 100644 engineer_middleware/include/engineer_middleware/moveit_exchange.h diff --git a/engineer_middleware/CMakeLists.txt b/engineer_middleware/CMakeLists.txt index 4c94c69..b6764bb 100644 --- a/engineer_middleware/CMakeLists.txt +++ b/engineer_middleware/CMakeLists.txt @@ -69,7 +69,8 @@ include_directories( add_executable(${PROJECT_NAME} src/middleware.cpp - src/engineer_middleware.cpp include/engineer_middleware/auto_exchange.h include/engineer_middleware/auto_exchange.h) + src/engineer_middleware.cpp include/engineer_middleware/auto_exchange.h include/engineer_middleware/auto_exchange.h + include/engineer_middleware/moveit_exchange.h) add_dependencies(${PROJECT_NAME} ${catkin_EXPORTED_TARGETS} diff --git a/engineer_middleware/include/engineer_middleware/moveit_exchange.h b/engineer_middleware/include/engineer_middleware/moveit_exchange.h new file mode 100644 index 0000000..d110df9 --- /dev/null +++ b/engineer_middleware/include/engineer_middleware/moveit_exchange.h @@ -0,0 +1,104 @@ +// +// Created by cch on 24-4-27. +// + +#pragma once +#include +#include +#include + +namespace moveit_exchange +{ +enum States +{ + FIND, + CHASSIS_APPROACH, + CHASSIS_POMPENSATE, + SPHERE, + LINE, + REACH +}; + +class SingleDirectionMove +{ +public: + std::string name; + double tolerance{}, start_vel{}, offset_refer_exchanger{}, max_vel{}, error{}, pid_value{}; + control_toolbox::Pid pid; + void init(XmlRpc::XmlRpcValue& config, std::string config_name, ros::NodeHandle& nh) + { + name = config_name; + error = 1e10; + max_vel = config.hasMember("max_vel") ? (double)config["max_vel"] : 1e10; + start_vel = config.hasMember("start_vel") ? (double)config["start_vel"] : 0.; + tolerance = config.hasMember("tolerance") ? (double)config["tolerance"] : 1e10; + offset_refer_exchanger = config.hasMember("offset_refer_exchanger") ? (double)config["offset_refer_exchanger"] : 0.; + ros::NodeHandle pid_config = ros::NodeHandle(nh, name); + pid.init(ros::NodeHandle(pid_config, "pid")); + } + bool isFinish() const + { + return abs(error) <= tolerance; + } + double computerVel(ros::Duration dt) + { + double vel = start_vel + abs(pid.computeCommand(error, dt)); + int direction = (error > 0) ? 1 : -1; + return abs(vel) >= max_vel ? direction * max_vel : direction * vel; + } + void getPidValue(ros::Duration dt) + { + double vel = start_vel + abs(pid.computeCommand(error, dt)); + int direction = (error > 0) ? 1 : -1; + pid_value = abs(vel) >= max_vel ? direction * max_vel : direction * vel; + } +}; + +class ChassisMotion +{ +public: + enum AdjustStates + { + SET_GOAL, + ADJUST_X, + ADJUST_Y, + ADJUST_YAW, + FINISH + }; + ChassisMotion(XmlRpc::XmlRpcValue& config, tf2_ros::Buffer& tf_buffer, ros::NodeHandle& nh) + : tf_buffer_(tf_buffer), nh_(nh) + { + state_ = SET_GOAL; + last_state_ = state_; + x_.init(config["x"], "x", nh); + y_.init(config["y"], "y", nh); + yaw_.init(config["yaw"], "yaw", nh); + } + +private: + void initComputeValue() + { + chassis_vel_cmd_.linear.x = 0; + chassis_vel_cmd_.linear.y = 0; + chassis_vel_cmd_.linear.z = 0; + chassis_vel_cmd_.angular.x = 0; + chassis_vel_cmd_.angular.y = 0; + chassis_vel_cmd_.angular.z = 0; + } + + AdjustStates state_, last_state_; + tf2_ros::Buffer& tf_buffer_; + ros::NodeHandle nh_{}; + SingleDirectionMove x_, y_, yaw_; + geometry_msgs::Twist chassis_vel_cmd_{}; +}; + +class FindExchanger +{ +}; + +class MoveitExchange +{ +}; + +} // namespace moveit_exchange From d50b6991109522bf1aa38c8f270dc8766b5f01cf Mon Sep 17 00:00:00 2001 From: cc0h <7921166012@qq.com> Date: Mon, 29 Apr 2024 22:48:56 +0800 Subject: [PATCH 14/80] Adjust chassis motion pid. --- engineer_middleware/config/engineer.yaml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/engineer_middleware/config/engineer.yaml b/engineer_middleware/config/engineer.yaml index 73c761e..fb1caf4 100644 --- a/engineer_middleware/config/engineer.yaml +++ b/engineer_middleware/config/engineer.yaml @@ -4,6 +4,6 @@ chassis: y: 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: 5., i: 0.0, d: 0.05, i_clamp_max: 0, i_clamp_min: 0, antiwindup: true, publish_state: true } + pid: { p: 5., i: 0.0, d: 0.0, i_clamp_max: 0, i_clamp_min: 0, antiwindup: true, publish_state: true } yaw_start_threshold: 0.05 max_vel: 0.1 From c678b18b80730de4f998a07a7ad7671fd74a67c6 Mon Sep 17 00:00:00 2001 From: cc0h <7921166012@qq.com> Date: Mon, 29 Apr 2024 23:49:22 +0800 Subject: [PATCH 15/80] change home step into 2 stages. --- engineer_middleware/config/steps_list.yaml | 248 +++++++++++++++++---- 1 file changed, 201 insertions(+), 47 deletions(-) diff --git a/engineer_middleware/config/steps_list.yaml b/engineer_middleware/config/steps_list.yaml index ff0f46b..6e97f02 100644 --- a/engineer_middleware/config/steps_list.yaml +++ b/engineer_middleware/config/steps_list.yaml @@ -46,7 +46,7 @@ common: big_up_pos: &JOINT1_BIG_UP_POS 0.320 big_ready_store: &JOINT1_BIG_READY_STORE - 0.233 + 0.133 home: zero_stone_position: &JOINT1_ZERO_STONE_POSITION 0.01 @@ -60,15 +60,17 @@ common: joint2: mechanical: back_position: &JOINT2_BACK_POSITION - 0.01 + 0.047 get_ore_position: &JOINT2_GET_ORE_POSITION 0.1 furthest_position: &JOINT2_FURTHEST_POSITION 0.232 exchange_position: &JOINT2_EXCHANGE_POSITION - 0.07 + 0.105 small_ready: &JOINT2_SMALL_READY - 0.034 + 0.029 + small_store: &JOINT2_SMALL_STORE + 0.044 bin_get_stone: &JOINT2_BIN_GET_STONE 0.087 back_bin_position: &JOINT2_BACK_BIN_POSITION @@ -78,7 +80,7 @@ common: big_arm_up: &JOINT2_BIG_ARM_UP 0.142 big_ready_store: &JOINT2_BIG_READY_STORE - 0.03 + 0.055 joint3: mechanical: @@ -109,7 +111,9 @@ common: big_pull_out: &JOINT3_BIG_PULL_OUT 0.188 big_arm_up: &JOINT3_BIG_ARM_UP - 0.267 + 0.285 + exchange: &JOINT3_EXCHANGE + 0.081 joint4: mechanical: @@ -125,6 +129,8 @@ common: -1.497 big_store: &JOINT4_BIG_STORE -1.525 + exchange: &JOINT4_EXCHANGE + -0.086 joint5: mechanical: mid_position: &JOINT5_MID_POSITION @@ -147,6 +153,8 @@ common: -0.001 small_store: &JOINT5_SMALL_STORE 1.615 + exchange: &JOINT5_EXCHANGE + 1.805 joint6: mechanical: @@ -172,13 +180,15 @@ common: -1.519 big_ready_store: &JOINT6_BIG_READY_STORE -1.561 + exchange: &JOINT6_EXCHANGE + 0.139 joint7: mechanical: mid_position: &JOINT7_MID_POSITION 0.044 left_90_position: &JOINT7_L90_POSITION - -1.59 + -1.56 right_90_position: &JOINT7_R90_POSITION 1.62 big_pos: &JOINT7_BIG_POS @@ -189,32 +199,33 @@ common: 0.012 small_ready_store: &JOINT7_SMALL_READY_STORE 1.722 + exchange: &JOINT7_EXCHANGE + -1.771 gimbal: side_pos: &SIDE_POS frame: gimbal_lifter - position: [ 0.001 , -3, 0. ] + position: [ 0.001 , -3., 0. ] front_pos: &FRONT_POS frame: gimbal_lifter - position: [ 2, 0.001, 0. ] + 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 - small_island: &LIFTER_SMALL_ISLAND - 0.05 mid_pos: &MID_POS 0.14 - big_island: &LIFTER_BIG_ISLAND - 0.230 two_stone_pos: &TWO_STONE_POS 0.05 tallest_pos: &TALLEST_POS 0.28 - + big_island: &LIFTER_BIG_ISLAND + 0.230 + small_island: &LIFTER_SMALL_ISLAND + 0.05 gripper: open_gripper: &OPEN_GRIPPER pin: 0 @@ -224,12 +235,20 @@ common: state: false arm: + pre_home: &PRE_HOME + joints: [ *JOINT1_ZERO_STONE_POSITION, *JOINT2_BACK_POSITION, *JOINT3_HOME_POS, "KEEP", *JOINT5_MID_POSITION, *JOINT6_UP_POSITION, *JOINT7_MID_POSITION ] + common: + <<: *NORMALLY + tolerance: + <<: *SMALL_TOLERANCE + home: &HOME 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 + <<: *SMALL_TOLERANCE + chassis: @@ -249,6 +268,22 @@ common: chassis_tolerance_angular: 0.2 common: timeout: 2. + chassis_90: &CHASSIS_TURN + frame: base_link + position: [ 0., 0. ] + yaw: -1.40 + chassis_tolerance_position: 0.1 + chassis_tolerance_angular: 0.2 + common: + timeout: 2. + chassis_back: &CHASSIS_BACK + frame: base_link + position: [ -0.4, 0. ] + yaw: 0.0 + chassis_tolerance_position: 0.1 + chassis_tolerance_angular: 0.2 + common: + timeout: 2. ore_rotator: ready_pos: &READY_POS @@ -276,7 +311,7 @@ steps_list: <<: *FRONT_POS - step: "move arm" arm: - joints: [*JOINT1_DOWN_POSITION, *JOINT2_EXCHANGE_POSITION, *JOINT3_MID_POSITION, *JOINT4_MID_POSITION, *JOINT5_MID_POSITION, *JOINT6_UP_POSITION, *JOINT7_MID_POSITION] + joints: [*JOINT1_DOWN_POSITION, *JOINT2_EXCHANGE_POSITION, *JOINT3_EXCHANGE, *JOINT4_EXCHANGE, *JOINT5_EXCHANGE, *JOINT6_EXCHANGE, *JOINT7_EXCHANGE] common: <<: *NORMALLY tolerance: @@ -316,6 +351,9 @@ steps_list: - step: "ORE_LIFTER_UP" ore_lifter: target: *BIN_DOWN_POS + - step: "pre_home" + arm: + <<: *PRE_HOME - step: "home" arm: <<: *HOME @@ -351,6 +389,9 @@ steps_list: - step: "OPEN_GRIPPER" gripper: <<: *OPEN_GRIPPER + - step: "gimbal lifter ready" + gimbal_lifter: + target: *LIFTER_SMALL_ISLAND ONE_STONE_SMALL_ISLAND0: - step: "GET_STONE" arm: @@ -389,14 +430,14 @@ steps_list: <<: *NORMAL_TOLERANCE - step: "READY_STORE" arm: - joints: [ *JOINT1_SMALL_READY_STORE, *JOINT2_SMALL_READY, *JOINT3_SMALL_TURN, *JOINT4_SMALL_TURN, *JOINT5_SMALL_UP_STONE, *JOINT6_SMALL_READY_STORE, *JOINT7_SMALL_READY_STORE ] + joints: [ *JOINT1_SMALL_READY_STORE, *JOINT2_SMALL_STORE, *JOINT3_SMALL_TURN, *JOINT4_SMALL_TURN, *JOINT5_SMALL_UP_STONE, *JOINT6_SMALL_READY_STORE, *JOINT7_SMALL_READY_STORE ] common: <<: *NORMALLY tolerance: <<: *NORMAL_TOLERANCE - step: "STORE" arm: - joints: [ *JOINT1_SMALL_READY_STORE, *JOINT2_SMALL_READY, *JOINT3_SMALL_TURN, *JOINT4_SMALL_TURN, *JOINT5_SMALL_STORE, *JOINT6_SMALL_READY_STORE, *JOINT7_SMALL_READY_STORE ] + joints: [ *JOINT1_SMALL_READY_STORE, *JOINT2_SMALL_STORE, *JOINT3_SMALL_TURN, *JOINT4_SMALL_TURN, *JOINT5_SMALL_STORE, *JOINT6_SMALL_READY_STORE, *JOINT7_SMALL_READY_STORE ] common: <<: *SLOWLY tolerance: @@ -412,6 +453,15 @@ steps_list: <<: *SLOWLY tolerance: <<: *NORMAL_TOLERANCE + - step: "CHASSIS_BACK" + chassis: + <<: *CHASSIS_BACK + - step: "CHASSIS_TURN" + chassis: + <<: *CHASSIS_TURN + - step: "pre_home" + arm: + <<: *PRE_HOME - step: "home" arm: <<: *HOME @@ -446,7 +496,7 @@ steps_list: <<: *OPEN_GRIPPER - step: "gimbal lifter ready" gimbal_lifter: - target: *BUTTON_POS + target: *LIFTER_SMALL_ISLAND TWO_STONE_SMALL_ISLAND0: - step: "GET_STONE" arm: @@ -485,14 +535,14 @@ steps_list: <<: *NORMAL_TOLERANCE - step: "READY_STORE" arm: - joints: [ *JOINT1_SMALL_READY_STORE, *JOINT2_SMALL_READY, *JOINT3_SMALL_TURN, *JOINT4_SMALL_TURN, *JOINT5_SMALL_UP_STONE, *JOINT6_SMALL_READY_STORE, *JOINT7_SMALL_READY_STORE ] + joints: [ *JOINT1_SMALL_READY_STORE, *JOINT2_SMALL_STORE, *JOINT3_SMALL_TURN, *JOINT4_SMALL_TURN, *JOINT5_SMALL_UP_STONE, *JOINT6_SMALL_READY_STORE, *JOINT7_SMALL_READY_STORE ] common: <<: *NORMALLY tolerance: <<: *NORMAL_TOLERANCE - step: "STORE" arm: - joints: [ *JOINT1_SMALL_READY_STORE, *JOINT2_SMALL_READY, *JOINT3_SMALL_TURN, *JOINT4_SMALL_TURN, *JOINT5_SMALL_STORE, *JOINT6_SMALL_READY_STORE, *JOINT7_SMALL_READY_STORE ] + joints: [ *JOINT1_SMALL_READY_STORE, *JOINT2_SMALL_STORE, *JOINT3_SMALL_TURN, *JOINT4_SMALL_TURN, *JOINT5_SMALL_STORE, *JOINT6_SMALL_READY_STORE, *JOINT7_SMALL_READY_STORE ] common: <<: *SLOWLY tolerance: @@ -573,14 +623,14 @@ steps_list: <<: *NORMAL_TOLERANCE - step: "READY_STORE" arm: - joints: [ *JOINT1_SMALL_READY_STORE, *JOINT2_SMALL_READY, *JOINT3_SMALL_TURN, *JOINT4_SMALL_TURN, *JOINT5_SMALL_UP_STONE, *JOINT6_SMALL_READY_STORE, *JOINT7_SMALL_READY_STORE ] + joints: [ *JOINT1_SMALL_READY_STORE, *JOINT2_SMALL_STORE, *JOINT3_SMALL_TURN, *JOINT4_SMALL_TURN, *JOINT5_SMALL_UP_STONE, *JOINT6_SMALL_READY_STORE, *JOINT7_SMALL_READY_STORE ] common: <<: *NORMALLY tolerance: <<: *NORMAL_TOLERANCE - step: "STORE" arm: - joints: [ *JOINT1_SMALL_READY_STORE, *JOINT2_SMALL_READY, *JOINT3_SMALL_TURN, *JOINT4_SMALL_TURN, *JOINT5_SMALL_STORE, *JOINT6_SMALL_READY_STORE, *JOINT7_SMALL_READY_STORE ] + joints: [ *JOINT1_SMALL_READY_STORE, *JOINT2_SMALL_STORE, *JOINT3_SMALL_TURN, *JOINT4_SMALL_TURN, *JOINT5_SMALL_STORE, *JOINT6_SMALL_READY_STORE, *JOINT7_SMALL_READY_STORE ] common: <<: *SLOWLY tolerance: @@ -595,6 +645,15 @@ steps_list: <<: *SLOWLY tolerance: <<: *NORMAL_TOLERANCE + - step: "CHASSIS_BACK" + chassis: + <<: *CHASSIS_BACK + - step: "CHASSIS_TURN" + chassis: + <<: *CHASSIS_TURN + - step: "pre_home" + arm: + <<: *PRE_HOME - step: "home" arm: <<: *HOME @@ -662,14 +721,14 @@ steps_list: <<: *NORMAL_TOLERANCE - step: "READY_STORE" arm: - joints: [ *JOINT1_SMALL_READY_STORE, *JOINT2_SMALL_READY, *JOINT3_SMALL_TURN, *JOINT4_SMALL_TURN, *JOINT5_SMALL_UP_STONE, *JOINT6_SMALL_READY_STORE, *JOINT7_SMALL_READY_STORE ] + joints: [ *JOINT1_SMALL_READY_STORE, *JOINT2_SMALL_STORE, *JOINT3_SMALL_TURN, *JOINT4_SMALL_TURN, *JOINT5_SMALL_UP_STONE, *JOINT6_SMALL_READY_STORE, *JOINT7_SMALL_READY_STORE ] common: <<: *NORMALLY tolerance: <<: *NORMAL_TOLERANCE - step: "STORE" arm: - joints: [ *JOINT1_SMALL_READY_STORE, *JOINT2_SMALL_READY, *JOINT3_SMALL_TURN, *JOINT4_SMALL_TURN, *JOINT5_SMALL_STORE, *JOINT6_SMALL_READY_STORE, *JOINT7_SMALL_READY_STORE ] + joints: [ *JOINT1_SMALL_READY_STORE, *JOINT2_SMALL_STORE, *JOINT3_SMALL_TURN, *JOINT4_SMALL_TURN, *JOINT5_SMALL_STORE, *JOINT6_SMALL_READY_STORE, *JOINT7_SMALL_READY_STORE ] common: <<: *SLOWLY tolerance: @@ -750,14 +809,14 @@ steps_list: <<: *NORMAL_TOLERANCE - step: "READY_STORE" arm: - joints: [ *JOINT1_SMALL_READY_STORE, *JOINT2_SMALL_READY, *JOINT3_SMALL_TURN, *JOINT4_SMALL_TURN, *JOINT5_SMALL_UP_STONE, *JOINT6_SMALL_READY_STORE, *JOINT7_SMALL_READY_STORE ] + joints: [ *JOINT1_SMALL_READY_STORE, *JOINT2_SMALL_STORE, *JOINT3_SMALL_TURN, *JOINT4_SMALL_TURN, *JOINT5_SMALL_UP_STONE, *JOINT6_SMALL_READY_STORE, *JOINT7_SMALL_READY_STORE ] common: <<: *NORMALLY tolerance: <<: *NORMAL_TOLERANCE - step: "STORE" arm: - joints: [ *JOINT1_SMALL_READY_STORE, *JOINT2_SMALL_READY, *JOINT3_SMALL_TURN, *JOINT4_SMALL_TURN, *JOINT5_SMALL_STORE, *JOINT6_SMALL_READY_STORE, *JOINT7_SMALL_READY_STORE ] + joints: [ *JOINT1_SMALL_READY_STORE, *JOINT2_SMALL_STORE, *JOINT3_SMALL_TURN, *JOINT4_SMALL_TURN, *JOINT5_SMALL_STORE, *JOINT6_SMALL_READY_STORE, *JOINT7_SMALL_READY_STORE ] common: <<: *SLOWLY tolerance: @@ -834,14 +893,14 @@ steps_list: <<: *NORMAL_TOLERANCE - step: "READY_STORE(THIRD_STONE)" arm: - joints: [ *JOINT1_SMALL_READY_STORE_THIRD, *JOINT2_SMALL_READY, *JOINT3_SMALL_TURN, *JOINT4_SMALL_TURN, *JOINT5_SMALL_UP_STONE, *JOINT6_SMALL_READY_STORE, *JOINT7_SMALL_READY_STORE ] + joints: [ *JOINT1_SMALL_READY_STORE_THIRD, *JOINT2_SMALL_STORE, *JOINT3_SMALL_TURN, *JOINT4_SMALL_TURN, *JOINT5_SMALL_UP_STONE, *JOINT6_SMALL_READY_STORE, *JOINT7_SMALL_READY_STORE ] common: <<: *NORMALLY tolerance: <<: *NORMAL_TOLERANCE - step: "STORE" arm: - joints: [ *JOINT1_SMALL_READY_STORE_THIRD, *JOINT2_SMALL_READY, *JOINT3_SMALL_TURN, *JOINT4_SMALL_TURN, *JOINT5_SMALL_STORE, *JOINT6_SMALL_READY_STORE, *JOINT7_SMALL_READY_STORE ] + joints: [ *JOINT1_SMALL_READY_STORE_THIRD, *JOINT2_SMALL_STORE, *JOINT3_SMALL_TURN, *JOINT4_SMALL_TURN, *JOINT5_SMALL_STORE, *JOINT6_SMALL_READY_STORE, *JOINT7_SMALL_READY_STORE ] common: <<: *SLOWLY tolerance: @@ -902,13 +961,30 @@ steps_list: <<: *NORMALLY tolerance: <<: *NORMAL_TOLERANCE - - step: "exchange pos" + - step: "exchange pos1" arm: - joints: [ *JOINT1_DOWN_POSITION, *JOINT2_EXCHANGE_POSITION, *JOINT3_MID_POSITION, *JOINT4_MID_POSITION, *JOINT5_MID_POSITION, *JOINT6_UP_POSITION, *JOINT7_MID_POSITION ] + joints: [ *JOINT1_DOWN_POSITION, *JOINT2_EXCHANGE_POSITION, "KEEP", "KEEP", "KEEP", "KEEP", "KEEP" ] common: <<: *NORMALLY tolerance: <<: *NORMAL_TOLERANCE + - step: "exchange pos2" + arm: + joints: [ *JOINT1_DOWN_POSITION, *JOINT2_EXCHANGE_POSITION, *JOINT3_EXCHANGE, *JOINT4_EXCHANGE, *JOINT5_MID_POSITION, *JOINT6_EXCHANGE, *JOINT7_MID_POSITION ] + common: + <<: *NORMALLY + tolerance: + <<: *NORMAL_TOLERANCE + - step: "exchange pos2" + arm: + joints: [ *JOINT1_DOWN_POSITION, *JOINT2_EXCHANGE_POSITION, *JOINT3_EXCHANGE, *JOINT4_EXCHANGE, *JOINT5_EXCHANGE, *JOINT6_EXCHANGE, *JOINT7_EXCHANGE] + common: + <<: *QUICKLY + tolerance: + <<: *NORMAL_TOLERANCE + - step: "ORE_LIFTER_DOWN" + ore_lifter: + target: *BIN_DOWN_POS - step: "gimbal lifter tallest" gimbal_lifter: target: *TALLEST_POS @@ -954,13 +1030,30 @@ steps_list: <<: *NORMALLY tolerance: <<: *NORMAL_TOLERANCE - - step: "exchange pos" + - step: "exchange pos1" + arm: + joints: [ *JOINT1_DOWN_POSITION, *JOINT2_EXCHANGE_POSITION, "KEEP", "KEEP", "KEEP", "KEEP", "KEEP" ] + common: + <<: *NORMALLY + tolerance: + <<: *NORMAL_TOLERANCE + - step: "exchange pos2" arm: - joints: [ *JOINT1_DOWN_POSITION, *JOINT2_EXCHANGE_POSITION, *JOINT3_MID_POSITION, *JOINT4_MID_POSITION, *JOINT5_MID_POSITION, *JOINT6_UP_POSITION, *JOINT7_MID_POSITION ] + joints: [ *JOINT1_DOWN_POSITION, *JOINT2_EXCHANGE_POSITION, *JOINT3_EXCHANGE, *JOINT4_EXCHANGE, *JOINT5_MID_POSITION, *JOINT6_EXCHANGE, *JOINT7_MID_POSITION ] common: <<: *NORMALLY tolerance: <<: *NORMAL_TOLERANCE + - step: "exchange pos2" + arm: + joints: [ *JOINT1_DOWN_POSITION, *JOINT2_EXCHANGE_POSITION, *JOINT3_EXCHANGE, *JOINT4_EXCHANGE, *JOINT5_EXCHANGE, *JOINT6_EXCHANGE, *JOINT7_EXCHANGE ] + common: + <<: *QUICKLY + tolerance: + <<: *NORMAL_TOLERANCE + - step: "ORE_LIFTER_DOWN" + ore_lifter: + target: *BIN_DOWN_POS - step: "gimbal lifter tallest" gimbal_lifter: target: *TALLEST_POS @@ -987,10 +1080,10 @@ steps_list: <<: *NORMAL_TOLERANCE - step: "ORE_ROTATOR_READY" ore_rotator: - target: *READY_POS - - step: "ORE_LIFTER_READY" - ore_lifter: - target: *BIN_MID_POS + target: *EXCHANGE_POS +# - step: "ORE_LIFTER_READY" +# ore_lifter: +# target: *BIN_MID_POS - step: "gimbal lifter" gimbal_lifter: target: *LIFTER_BIG_ISLAND @@ -1038,14 +1131,14 @@ steps_list: <<: *NORMAL_TOLERANCE - step: "READY_TO_STORE" arm: - joints: [ *JOINT1_BIG_READY_STORE, *JOINT2_BIG_READY_STORE, *JOINT3_BIG_ARM_UP, *JOINT4_BIG_STORE, *JOINT5_BIG_READY_STORE, *JOINT6_BIG_READY_STORE, *JOINT7_BIG_READY_STORE ] + joints: [ *JOINT1_BIG_READY_STORE, *JOINT2_BIG_READY_STORE, *JOINT3_BIG_ARM_UP, *JOINT4_BIG_STORE, *JOINT5_BIG_READY_STORE, *JOINT6_BIG_READY_STORE, *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_BIG_STORE, *JOINT5_BIG_STORE, *JOINT6_BIG_READY_STORE, *JOINT7_BIG_READY_STORE ] + joints: [ *JOINT1_BIG_READY_STORE, *JOINT2_BIG_READY_STORE, *JOINT3_BIG_ARM_UP, *JOINT4_BIG_STORE, *JOINT5_BIG_STORE, *JOINT6_BIG_READY_STORE, *JOINT7_BIG_POS ] common: <<: *SLOWLY tolerance: @@ -1061,12 +1154,22 @@ steps_list: <<: *NORMALLY tolerance: <<: *NORMAL_TOLERANCE + - step: "pre_home" + arm: + <<: *PRE_HOME - step: "home" arm: <<: *HOME - step: "ORE_LIFTER_DOWN" ore_lifter: target: *BIN_DOWN_POS + - step: "ORE_ROTATOR_RESET" + ore_rotator: + target: *READY_POS + - step: "EX_ARM_RESET" + extend_arm: + front: *EX_CLOSE + back: *EX_CLOSE - step: "add stone" stone_num: change: "GOLD" @@ -1082,7 +1185,7 @@ steps_list: - step: "open ex arm" extend_arm: front: *EX_OPEN - + back: *EX_CLOSE - 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 ] @@ -1092,10 +1195,10 @@ steps_list: <<: *NORMAL_TOLERANCE - step: "ORE_ROTATOR_READY" ore_rotator: - target: *READY_POS - - step: "ORE_LIFTER_READY" - ore_lifter: - target: *BIN_MID_POS + target: *EXCHANGE_POS +# - step: "ORE_LIFTER_READY" +# ore_lifter: +# target: *BIN_MID_POS - step: "gimbal lifter" gimbal_lifter: target: *LIFTER_BIG_ISLAND @@ -1144,14 +1247,14 @@ steps_list: <<: *NORMAL_TOLERANCE - step: "READY_TO_STORE" arm: - joints: [ *JOINT1_BIG_READY_STORE, *JOINT2_BIG_READY_STORE, *JOINT3_BIG_ARM_UP, *JOINT4_BIG_STORE, *JOINT5_BIG_READY, *JOINT6_BIG_READY_STORE, *JOINT7_BIG_READY_STORE ] + joints: [ *JOINT1_BIG_READY_STORE, *JOINT2_BIG_READY_STORE, *JOINT3_BIG_ARM_UP, *JOINT4_BIG_STORE, *JOINT5_BIG_READY_STORE, *JOINT6_BIG_READY_STORE, *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_BIG_STORE, *JOINT5_BIG_STORE, *JOINT6_BIG_READY_STORE, *JOINT7_BIG_READY_STORE ] + joints: [ *JOINT1_BIG_READY_STORE, *JOINT2_BIG_READY_STORE, *JOINT3_BIG_ARM_UP, *JOINT4_BIG_STORE, *JOINT5_BIG_STORE, *JOINT6_BIG_READY_STORE, *JOINT7_BIG_POS ] common: <<: *SLOWLY tolerance: @@ -1166,18 +1269,59 @@ steps_list: <<: *NORMALLY tolerance: <<: *NORMAL_TOLERANCE + - step: "pre_home" + arm: + <<: *PRE_HOME - step: "home" arm: <<: *HOME - step: "ORE_LIFTER_DOWN" ore_lifter: target: *BIN_DOWN_POS + - step: "ORE_ROTATOR_RESET" + ore_rotator: + target: *READY_POS + - step: "EX_ARM_RESET" + extend_arm: + front: *EX_CLOSE + back: *EX_CLOSE - step: "add stone" stone_num: change: "GOLD" - step: "gimbal front" gimbal: <<: *FRONT_POS + ##########GIMBAL_LIFTER########## + ZERO_STONE_GIMBAL_LIFTER: + - step: "gimbal lifter zero stone pos" + gimbal_lifter: + target: *BUTTON_POS + ONE_STONE_GIMBAL_LIFTER: + - step: "gimbal lifter one stone pos" + gimbal_lifter: + target: *MID_POS + TWO_STONE_GIMBAL_LIFTER: + - step: "gimbal lifter zero stone pos" + gimbal_lifter: + target: *TWO_STONE_POS + TALLEST_GIMBAL_LIFTER: + - step: "gimbal lifter tallest pos" + gimbal_lifter: + target: *TALLEST_POS + + ##########ORE_LIFTER############### + ORE_LIFTER_DOWN: + - step: "ore lifter down" + ore_lifter: + target: *BIN_DOWN_POS + ORE_LIFTER_MID: + - step: "ore lifter mid" + ore_lifter: + target: *BIN_MID_POS + ORE_LIFTER_UP: + - step: "ore lifter up" + ore_lifter: + target: *BIN_UP_POS #################################################### TEST ########################################################## @@ -1279,3 +1423,13 @@ steps_list: - step: "gimbal tall" gimbal_lifter: target: *TALLEST_POS + + CHASSIS_TEST1: + - step: "chassis turn" + chassis: + <<: *CHASSIS_TURN + + CHASSIS_TEST2: + - step: "chassis back" + chassis: + <<: *CHASSIS_BACK From a316fe8cd146dca8b95fbbd7391439c511d4f931 Mon Sep 17 00:00:00 2001 From: cc0h <7921166012@qq.com> Date: Tue, 30 Apr 2024 01:33:20 +0800 Subject: [PATCH 16/80] change servo direction --- engineer_arm_config/config/servo.yaml | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/engineer_arm_config/config/servo.yaml b/engineer_arm_config/config/servo.yaml index 298263e..70b7b47 100644 --- a/engineer_arm_config/config/servo.yaml +++ b/engineer_arm_config/config/servo.yaml @@ -7,8 +7,8 @@ use_gazebo: true # Whether the robot is started in a Gazebo simulation environme command_in_type: "speed_units" # "unitless"> in the range [-1:1], as if from joystick. "speed_units"> cmds are in m/s and rad/s scale: # Scale parameters are only used if command_in_type=="unitless" - linear: 0.6 # Max linear velocity. Meters per publish_period. Unit is [m/s]. Only used for Cartesian commands. - rotational: 0.3 # Max angular velocity. Rads per publish_period. Unit is [rad/s]. Only used for Cartesian commands. + linear: 0.4 # Max linear velocity. Meters per publish_period. Unit is [m/s]. Only used for Cartesian commands. + rotational: 0.2 # Max angular velocity. Rads per publish_period. Unit is [rad/s]. Only used for Cartesian commands. # Max joint angular/linear velocity. Rads or Meters per publish period. Only used for joint commands on joint_command_in_topic. joint: 0.005 low_pass_filter_coeff: 1. # Larger --> trust the filtered data more, trust the measurements less. @@ -32,8 +32,8 @@ move_group_name: engineer_arm # Often 'manipulator' or 'arm' planning_frame: base_link # The MoveIt planning frame. Often 'base_link' or 'world' ## Other frames -ee_frame_name: link7 # The name of the end effector link, used to return the EE pose -robot_link_command_frame: link7 # commands must be given in the frame of a robot link. Usually either the base or end effector +ee_frame_name: tools_link # The name of the end effector link, used to return the EE pose +robot_link_command_frame: tools_link # commands must be given in the frame of a robot link. Usually either the base or end effector ## Stopping behaviour incoming_command_timeout: 0.1 # Stop servoing if X seconds elapse without a new command From 8eca8f67a290274e5503f56c0d213b1f490e2139 Mon Sep 17 00:00:00 2001 From: cc0h <7921166012@qq.com> Date: Tue, 30 Apr 2024 01:34:21 +0800 Subject: [PATCH 17/80] record. --- engineer_middleware/config/steps_list.yaml | 8 ++++++-- 1 file changed, 6 insertions(+), 2 deletions(-) diff --git a/engineer_middleware/config/steps_list.yaml b/engineer_middleware/config/steps_list.yaml index 6e97f02..7969fea 100644 --- a/engineer_middleware/config/steps_list.yaml +++ b/engineer_middleware/config/steps_list.yaml @@ -131,6 +131,10 @@ common: -1.525 exchange: &JOINT4_EXCHANGE -0.086 + pre_home: &JOINT4_PRE_HOME + -0.91 + + joint5: mechanical: mid_position: &JOINT5_MID_POSITION @@ -236,7 +240,7 @@ common: arm: pre_home: &PRE_HOME - joints: [ *JOINT1_ZERO_STONE_POSITION, *JOINT2_BACK_POSITION, *JOINT3_HOME_POS, "KEEP", *JOINT5_MID_POSITION, *JOINT6_UP_POSITION, *JOINT7_MID_POSITION ] + joints: [ *JOINT1_ZERO_STONE_POSITION, *JOINT2_BACK_POSITION, *JOINT3_HOME_POS, *JOINT4_PRE_HOME, *JOINT5_MID_POSITION, *JOINT6_UP_POSITION, *JOINT7_MID_POSITION ] common: <<: *NORMALLY tolerance: @@ -311,7 +315,7 @@ steps_list: <<: *FRONT_POS - step: "move arm" arm: - joints: [*JOINT1_DOWN_POSITION, *JOINT2_EXCHANGE_POSITION, *JOINT3_EXCHANGE, *JOINT4_EXCHANGE, *JOINT5_EXCHANGE, *JOINT6_EXCHANGE, *JOINT7_EXCHANGE] + 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: From d21e50027671943c50d2b253950655bb4d8fd5f3 Mon Sep 17 00:00:00 2001 From: cc0h <7921166012@qq.com> Date: Wed, 1 May 2024 22:08:07 +0800 Subject: [PATCH 18/80] record. --- .../engineer_middleware/moveit_exchange.h | 98 ++++++++++++++++++- 1 file changed, 94 insertions(+), 4 deletions(-) diff --git a/engineer_middleware/include/engineer_middleware/moveit_exchange.h b/engineer_middleware/include/engineer_middleware/moveit_exchange.h index d110df9..7cca356 100644 --- a/engineer_middleware/include/engineer_middleware/moveit_exchange.h +++ b/engineer_middleware/include/engineer_middleware/moveit_exchange.h @@ -74,6 +74,60 @@ class ChassisMotion y_.init(config["y"], "y", nh); yaw_.init(config["yaw"], "yaw", nh); } + void init() + { + state_ = SET_GOAL; + initComputeValue(); + } + void stateMachine() + { + switch (state_) + { + case SET_GOAL: + { + setGoal(); + state_ = ADJUST_X; + } + break; + case ADJUST_X: + { + computeChassisVel(); + chassis_vel_cmd_.linear.y = 0.; + chassis_vel_cmd_.angular.z = 0.; + if (x_.isFinish()) + state_ = re_adjusted_ ? FINISH : ADJUST_Y; + } + break; + case ADJUST_Y: + { + computeChassisVel(); + chassis_vel_cmd_.linear.x = 0.; + chassis_vel_cmd_.angular.z = 0.; + if (y_.isFinish()) + state_ = ADJUST_YAW; + } + break; + case ADJUST_YAW: + { + computeChassisVel(); + chassis_vel_cmd_.linear.x = 0.; + chassis_vel_cmd_.linear.y = 0.; + if (yaw_.isFinish()) + { + state_ = SET_GOAL; + re_adjusted_ = true; + } + } + break; + case FINISH: + { + is_finish_ = true; + re_adjusted_ = false; + initComputeValue(); + } + break; + } + } private: void initComputeValue() @@ -85,16 +139,52 @@ class ChassisMotion chassis_vel_cmd_.angular.y = 0; chassis_vel_cmd_.angular.z = 0; } + void computeChassisVel() + { + geometry_msgs::TransformStamped current; + current = tf_buffer_.lookupTransform("map", "base_link", ros::Time(0)); + x_.error = chassis_target_.pose.position.x - current.transform.translation.x; + y_.error = chassis_target_.pose.position.y - current.transform.translation.y; + double roll, pitch, yaw_current, yaw_goal; + quatToRPY(current.transform.rotation, roll, pitch, yaw_current); + quatToRPY(chassis_target_.pose.orientation, roll, pitch, yaw_goal); + yaw_.error = angles::shortest_angular_distance(yaw_current, yaw_goal); + ros::Duration dt = ros::Time::now() - last_time_; + chassis_vel_cmd_.linear.x = x_.computerVel(dt); + chassis_vel_cmd_.linear.y = y_.computerVel(dt); + chassis_vel_cmd_.angular.z = yaw_.computerVel(dt); + + last_time_ = ros::Time::now(); + } + void setGoal() + { + geometry_msgs::TransformStamped base2exchange; + double roll, pitch, yaw; + base2exchange = tf_buffer_.lookupTransform("base_link", "exchanger", ros::Time(0)); + quatToRPY(base2exchange.transform.rotation, roll, pitch, yaw); + + double goal_x = base2exchange.transform.translation.x - x_.offset_refer_exchanger; + double goal_y = base2exchange.transform.translation.y - y_.offset_refer_exchanger; + double goal_yaw = yaw * yaw_.offset_refer_exchanger; + chassis_original_target_.pose.position.x = goal_x; + chassis_original_target_.pose.position.y = goal_y; + + tf2::Quaternion quat_tf; + quat_tf.setRPY(0, 0, goal_yaw); + geometry_msgs::Quaternion quat_msg = tf2::toMsg(quat_tf); + chassis_original_target_.pose.orientation = quat_msg; + chassis_target_ = chassis_original_target_; + tf2::doTransform(chassis_target_, chassis_target_, tf_buffer_.lookupTransform("map", "base_link", ros::Time(0))); + } + bool re_adjusted_{ false }, is_finish_{ false }; AdjustStates state_, last_state_; tf2_ros::Buffer& tf_buffer_; ros::NodeHandle nh_{}; + ros::Time last_time_; SingleDirectionMove x_, y_, yaw_; geometry_msgs::Twist chassis_vel_cmd_{}; -}; - -class FindExchanger -{ + geometry_msgs::PoseStamped chassis_target_{}, chassis_original_target_{}; }; class MoveitExchange From afc93b8fd154bdf50c6c59b4339137c52789db84 Mon Sep 17 00:00:00 2001 From: cc0h <7921166012@qq.com> Date: Thu, 2 May 2024 17:00:00 +0800 Subject: [PATCH 19/80] add ChassisTargetMotion,pass arm2base from JointMotion to ChassisTargetMotion. --- engineer_middleware/config/steps_list.yaml | 185 +++++++++++++++--- .../include/engineer_middleware/motion.h | 178 ++++++++++++----- .../include/engineer_middleware/points.h | 6 + .../include/engineer_middleware/step.h | 13 +- 4 files changed, 307 insertions(+), 75 deletions(-) diff --git a/engineer_middleware/config/steps_list.yaml b/engineer_middleware/config/steps_list.yaml index 7969fea..46a66f5 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.02 small_ready: &JOINT1_SMALL_READY - 0.480 + 0.487 small_down: &JOINT1_SMALL_DOWN 0.31 small_ready_store: &JOINT1_SMALL_READY_STORE @@ -37,8 +37,10 @@ common: 0.387 up_pos: &JOINT1_UP_POS 0.312 - bin_get_stone: &JOINT1_BIN_GET_STONE - 0.142 + bin_get_stone_left: &JOINT1_BIN_GET_STONE_LEFT + 0.232 + bin_get_stone_right: &JOINT1_BIN_GET_STONE_RIGHT + 0.144 big_ready: &JOINT1_BIG_READY 0.006 big_adjust_mid: &JOINT1_BIG_ADJUST_MID @@ -101,11 +103,11 @@ common: small_turn: &JOINT3_SMALL_TURN 0.257 bin_get_stone: &JOINT3_BIN_GET_STONE - 0.258 + 0.252 big_ready: &JOINT3_BIG_READY 0.071 get_big_mid: &JOINT3_GET_BIG_MID - -0.295 + -0.299 get_big_side: &JOINT3_GET_BIG_SIDE -0.199 big_pull_out: &JOINT3_BIG_PULL_OUT @@ -120,7 +122,7 @@ common: left_position: &JOINT4_L_POSITION 1.00 mid_position: &JOINT4_MID_POSITION - 0.001 + 0.021 right_position: &JOINT4_R_POSITION -1.554 small_ready: &JOINT4_SMALL_READY @@ -152,7 +154,7 @@ common: big_store: &JOINT5_BIG_STORE 1.49 small_ready: &JOINT5_SMALL_READY - -3.128 + -3.20 small_up_stone: &JOINT5_SMALL_UP_STONE -0.001 small_store: &JOINT5_SMALL_STORE @@ -205,6 +207,10 @@ common: 1.722 exchange: &JOINT7_EXCHANGE -1.771 + bin_get_left: &JOINT7_BIN_GET_LEFT + -1.526 + bin_get_right: &JOINT7_BIN_GET_RIGHT + 1.594 gimbal: side_pos: &SIDE_POS @@ -254,6 +260,16 @@ common: <<: *SMALL_TOLERANCE + chassis_target: + test: &TEST + frame: base_link + chassis_tolerance_position: 0.01 + chassis_tolerance_angular: 0.01 + offset: [0., 0.] + yaw_scale: 1 + target_frame: exchanger + common: + timeout: 2. chassis: chassis_left_315: &CHASSIS_LEFT_15 @@ -302,6 +318,10 @@ common: 0.1 bin_down_pos: &BIN_DOWN_POS 0.001 + bin_get_up: &BIN_GET_UP + 0.033 + bin_get_down: &BIN_GET_DOWN + 0.225 extend_arm: close: &EX_CLOSE 0.0 @@ -309,6 +329,11 @@ common: 2.6 steps_list: + TEST: + - step: "test" + chassis_target: + <<: *TEST + EXCHANGE_POS: - step: "gimbal look front" gimbal: @@ -924,7 +949,7 @@ steps_list: change: "SILVER" ######GET STONE FROM BIN######### - GET_DOWN_STONE_BIN: + GET_DOWN_STONE_LEFT: - step: "GIMBAL_READY" gimbal: <<: *FRONT_POS @@ -933,10 +958,10 @@ steps_list: target: *EXCHANGE_POS - step: "ORE_LIFTER_UP" ore_lifter: - target: *BIN_UP_POS + target: *BIN_GET_DOWN - 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 ] + joints: [ *JOINT1_BIN_GET_STONE_LEFT, *JOINT2_BIN_GET_STONE, *JOINT3_BIN_GET_STONE, *JOINT4_R_POSITION, *JOINT5_BIN_GET_STONE, *JOINT6_BIN_GET_STONE, *JOINT7_BIN_GET_LEFT ] common: <<: *NORMALLY tolerance: @@ -965,25 +990,69 @@ steps_list: <<: *NORMALLY tolerance: <<: *NORMAL_TOLERANCE - - step: "exchange pos1" + - step: "exchange pos" arm: - joints: [ *JOINT1_DOWN_POSITION, *JOINT2_EXCHANGE_POSITION, "KEEP", "KEEP", "KEEP", "KEEP", "KEEP" ] + 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: "exchange pos2" + - step: "ORE_LIFTER_DOWN" + ore_lifter: + target: *BIN_DOWN_POS + - step: "gimbal lifter tallest" + gimbal_lifter: + target: *TALLEST_POS + - step: "remove stone" + stone_num: + change: "-1" + + GET_DOWN_STONE_RIGHT: + - step: "GIMBAL_READY" + gimbal: + <<: *FRONT_POS + - step: "ORE_ROTATOR_READY" + ore_rotator: + target: *EXCHANGE_POS + - step: "ORE_LIFTER_UP" + ore_lifter: + target: *BIN_GET_DOWN + - step: "READY_TO_GET" arm: - joints: [ *JOINT1_DOWN_POSITION, *JOINT2_EXCHANGE_POSITION, *JOINT3_EXCHANGE, *JOINT4_EXCHANGE, *JOINT5_MID_POSITION, *JOINT6_EXCHANGE, *JOINT7_MID_POSITION ] + joints: [ *JOINT1_BIN_GET_STONE_RIGHT, *JOINT2_BIN_GET_STONE, *JOINT3_BIN_GET_STONE, *JOINT4_R_POSITION, *JOINT5_BIN_GET_STONE, *JOINT6_BIN_GET_STONE, *JOINT7_BIN_GET_RIGHT ] common: <<: *NORMALLY tolerance: <<: *NORMAL_TOLERANCE - - step: "exchange pos2" + - step: "OPEN_GRIPPER" + gripper: + <<: *OPEN_GRIPPER + - step: "JOINT2_BACK_GET_STONE" arm: - joints: [ *JOINT1_DOWN_POSITION, *JOINT2_EXCHANGE_POSITION, *JOINT3_EXCHANGE, *JOINT4_EXCHANGE, *JOINT5_EXCHANGE, *JOINT6_EXCHANGE, *JOINT7_EXCHANGE] + joints: [ "KEEP", *JOINT2_BACK_BIN_POSITION, "KEEP", "KEEP", "KEEP", "KEEP", "KEEP" ] common: - <<: *QUICKLY + <<: *SLOWLY + tolerance: + <<: *NORMAL_TOLERANCE + - step: "JOINT1_UP" + arm: + joints: [ *JOINT1_UP_POS, "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 + - step: "exchange pos" + arm: + 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: "ORE_LIFTER_DOWN" @@ -996,16 +1065,19 @@ steps_list: stone_num: change: "-1" - GET_UP_STONE_BIN: + GET_UP_STONE_LEFT: - step: "GIMBAL_READY" gimbal: <<: *FRONT_POS - step: "ORE_ROTATOR_READY" ore_rotator: target: *EXCHANGE_POS + - step: "ORE_LIFTER_READY" + ore_lifter: + target: *BIN_GET_UP - 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 ] + joints: [ *JOINT1_BIN_GET_STONE_LEFT, *JOINT2_BIN_GET_STONE, *JOINT3_BIN_GET_STONE, *JOINT4_R_POSITION, *JOINT5_BIN_GET_STONE, *JOINT6_BIN_GET_STONE, *JOINT7_BIN_GET_LEFT ] common: <<: *NORMALLY tolerance: @@ -1034,25 +1106,69 @@ steps_list: <<: *NORMALLY tolerance: <<: *NORMAL_TOLERANCE - - step: "exchange pos1" + - step: "exchange pos" arm: - joints: [ *JOINT1_DOWN_POSITION, *JOINT2_EXCHANGE_POSITION, "KEEP", "KEEP", "KEEP", "KEEP", "KEEP" ] + 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: "exchange pos2" + - step: "ORE_LIFTER_DOWN" + ore_lifter: + target: *BIN_DOWN_POS + - step: "gimbal lifter tallest" + gimbal_lifter: + target: *TALLEST_POS + - step: "remove stone" + stone_num: + change: "-1" + + GET_UP_STONE_RIGHT: + - step: "GIMBAL_READY" + gimbal: + <<: *FRONT_POS + - step: "ORE_ROTATOR_READY" + ore_rotator: + target: *EXCHANGE_POS + - step: "ORE_LIFTER_READY" + ore_lifter: + target: *BIN_GET_UP + - step: "READY_TO_GET" arm: - joints: [ *JOINT1_DOWN_POSITION, *JOINT2_EXCHANGE_POSITION, *JOINT3_EXCHANGE, *JOINT4_EXCHANGE, *JOINT5_MID_POSITION, *JOINT6_EXCHANGE, *JOINT7_MID_POSITION ] + joints: [ *JOINT1_BIN_GET_STONE_RIGHT, *JOINT2_BIN_GET_STONE, *JOINT3_BIN_GET_STONE, *JOINT4_R_POSITION, *JOINT5_BIN_GET_STONE, *JOINT6_BIN_GET_STONE, *JOINT7_BIN_GET_RIGHT ] common: <<: *NORMALLY tolerance: <<: *NORMAL_TOLERANCE - - step: "exchange pos2" + - step: "OPEN_GRIPPER" + gripper: + <<: *OPEN_GRIPPER + - step: "JOINT2_BACK_GET_STONE" arm: - joints: [ *JOINT1_DOWN_POSITION, *JOINT2_EXCHANGE_POSITION, *JOINT3_EXCHANGE, *JOINT4_EXCHANGE, *JOINT5_EXCHANGE, *JOINT6_EXCHANGE, *JOINT7_EXCHANGE ] + joints: [ "KEEP", *JOINT2_BACK_BIN_POSITION, "KEEP", "KEEP", "KEEP", "KEEP", "KEEP" ] common: - <<: *QUICKLY + <<: *SLOWLY + tolerance: + <<: *NORMAL_TOLERANCE + - step: "JOINT1_UP" + arm: + joints: [ *JOINT1_UP_POS, "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 + - step: "exchange pos" + arm: + 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: "ORE_LIFTER_DOWN" @@ -1328,6 +1444,23 @@ steps_list: target: *BIN_UP_POS #################################################### TEST ########################################################## + ARM_BACK_TEST: + - step: "arm back test" + arm: + joints: [ "KEEP", *JOINT2_BACK_POSITION, *JOINT3_HOME_POS, "KEEP", "KEEP", "KEEP", "KEEP" ] + record_arm2base: true + common: + <<: *NORMALLY + tolerance: + <<: *NORMAL_TOLERANCE + + CHASSIS_TARGET_TEST1: + - step: "test" + chassis: + frame: "base_link" + offset: [ 0., 0. ] + yaw_scale: 0.9 + target_frame: "exchanger" ########## EXCHANGE ############ OLD_EXCHANGE_SPHERE: diff --git a/engineer_middleware/include/engineer_middleware/motion.h b/engineer_middleware/include/engineer_middleware/motion.h index 44cd7f9..f216dcc 100644 --- a/engineer_middleware/include/engineer_middleware/motion.h +++ b/engineer_middleware/include/engineer_middleware/motion.h @@ -338,9 +338,6 @@ class SpaceEeMotion : public EndEffectorMotion final_target_.pose.orientation.z = tf_quaternion.z(); final_target_.header.frame_id = interface_.getPlanningFrame(); - double rolll, pitchh, yaww; - quatToRPY(final_target_.pose.orientation, rolll, pitchh, yaww); - ROS_INFO_STREAM("transformed roll: " << rolll); } catch (tf2::TransformException& ex) { @@ -384,8 +381,9 @@ class SpaceEeMotion : public EndEffectorMotion class JointMotion : public MoveitMotionBase { public: - JointMotion(XmlRpc::XmlRpcValue& motion, moveit::planning_interface::MoveGroupInterface& interface) - : MoveitMotionBase(motion, interface) + JointMotion(XmlRpc::XmlRpcValue& motion, moveit::planning_interface::MoveGroupInterface& interface, + tf2_ros::Buffer& tf_buffer) + : MoveitMotionBase(motion, interface), tf_buffer_(tf_buffer) { if (motion.hasMember("joints")) { @@ -408,9 +406,26 @@ class JointMotion : public MoveitMotionBase for (int i = 0; i < motion["tolerance"]["tolerance_joints"].size(); ++i) tolerance_joints_.push_back(xmlRpcGetDouble(motion["tolerance"]["tolerance_joints"], i)); } + if (motion.hasMember("record_arm2base")) + record_arm2base_ = bool(motion["record_arm2base"]); } bool move() override { + if (record_arm2base_) + { + try + { + arm2base.header.frame_id = "base_link"; + arm2base.header.stamp = ros::Time::now(); + arm2base.child_frame_id = "chassis_target"; + arm2base = tf_buffer_.lookupTransform("base_link", "joint4", ros::Time(0)); + } + catch (tf2::TransformException& ex) + { + ROS_WARN("%s", ex.what()); + return false; + } + } final_target_.clear(); if (target_.empty()) return false; @@ -431,6 +446,7 @@ class JointMotion : public MoveitMotionBase msg_.data = interface_.plan(plan).val; return (interface_.asyncExecute(plan) == moveit::planning_interface::MoveItErrorCode::SUCCESS); } + static geometry_msgs::TransformStamped arm2base; private: bool isReachGoal() override @@ -446,6 +462,8 @@ class JointMotion : public MoveitMotionBase return flag; } std::vector target_, final_target_, tolerance_joints_; + bool record_arm2base_{ false }; + tf2_ros::Buffer& tf_buffer_; }; template @@ -589,49 +607,6 @@ class GimbalMotion : public PublishMotion } }; -class ChassisMotion : public MotionBase -{ -public: - ChassisMotion(XmlRpc::XmlRpcValue& motion, ChassisInterface& interface) - : MotionBase(motion, interface) - { - chassis_tolerance_position_ = xmlRpcGetDouble(motion, "chassis_tolerance_position", 0.01); - chassis_tolerance_angular_ = xmlRpcGetDouble(motion, "chassis_tolerance_angular", 0.01); - if (motion.hasMember("frame")) - target_.header.frame_id = std::string(motion["frame"]); - if (motion.hasMember("position")) - { - target_.pose.position.x = xmlRpcGetDouble(motion["position"], 0); - target_.pose.position.y = xmlRpcGetDouble(motion["position"], 1); - } - if (motion.hasMember("yaw")) - { - tf2::Quaternion quat_tf; - quat_tf.setRPY(0, 0, motion["yaw"]); - geometry_msgs::Quaternion quat_msg = tf2::toMsg(quat_tf); - target_.pose.orientation = quat_msg; - } - } - bool move() override - { - interface_.setGoal(target_); - return true; - } - bool isFinish() override - { - return interface_.getErrorPos() < chassis_tolerance_position_ && - interface_.getErrorYaw() < chassis_tolerance_angular_; - } - void stop() override - { - interface_.stop(); - } - -private: - geometry_msgs::PoseStamped target_; - double chassis_tolerance_position_, chassis_tolerance_angular_; -}; - class ReversalMotion : public PublishMotion { public: @@ -728,4 +703,111 @@ class ExtendMotion : public PublishMotion double target_; }; +class ChassisMotion : public MotionBase +{ +public: + ChassisMotion(XmlRpc::XmlRpcValue& motion, ChassisInterface& interface) + : MotionBase(motion, interface) + { + chassis_tolerance_position_ = xmlRpcGetDouble(motion, "chassis_tolerance_position", 0.01); + chassis_tolerance_angular_ = xmlRpcGetDouble(motion, "chassis_tolerance_angular", 0.01); + if (motion.hasMember("frame")) + target_.header.frame_id = std::string(motion["frame"]); + if (motion.hasMember("position")) + { + target_.pose.position.x = xmlRpcGetDouble(motion["position"], 0); + target_.pose.position.y = xmlRpcGetDouble(motion["position"], 1); + } + if (motion.hasMember("yaw")) + { + tf2::Quaternion quat_tf; + quat_tf.setRPY(0, 0, motion["yaw"]); + geometry_msgs::Quaternion quat_msg = tf2::toMsg(quat_tf); + target_.pose.orientation = quat_msg; + } + } + bool move() override + { + interface_.setGoal(target_); + return true; + } + bool isFinish() override + { + return interface_.getErrorPos() < chassis_tolerance_position_ && + interface_.getErrorYaw() < chassis_tolerance_angular_; + } + void stop() override + { + interface_.stop(); + } + +protected: + geometry_msgs::PoseStamped target_; + double chassis_tolerance_position_, chassis_tolerance_angular_; +}; + +class ChassisTargetMotion : public ChassisMotion +{ +public: + ChassisTargetMotion(XmlRpc::XmlRpcValue& motion, ChassisInterface& interface, tf2_ros::Buffer& tf_buffer) + : ChassisMotion(motion, interface), tf_buffer_(tf_buffer) + { + x_offset_ = xmlRpcGetDouble(motion["offset"], 0); + y_offset_ = xmlRpcGetDouble(motion["offset"], 1); + yaw_scale_ = xmlRpcGetDouble(motion, "yaw_scale", 1); + move_target_ = std::string(motion["target_frame"]); + } + bool move() override + { + if (move_target_ == "arm") + { + try + { + double roll, pitch, yaw; + target_.pose.position.x = JointMotion::arm2base.transform.translation.x; + target_.pose.position.y = JointMotion::arm2base.transform.translation.y; + quatToRPY(JointMotion::arm2base.transform.rotation, roll, pitch, yaw); + tf2::Quaternion quat_tf; + quat_tf.setRPY(0, 0, yaw * yaw_scale_); + geometry_msgs::Quaternion quat_msg = tf2::toMsg(quat_tf); + target_.pose.orientation = quat_msg; + } + catch (tf2::TransformException& ex) + { + ROS_WARN("%s", ex.what()); + return false; + } + } + else + { + try + { + double roll, pitch, yaw; + geometry_msgs::TransformStamped target2base_link; + target2base_link = tf_buffer_.lookupTransform("base_link", move_target_, ros::Time(0)); + target_.pose.position.x = target2base_link.transform.translation.x; + target_.pose.position.y = target2base_link.transform.translation.y; + quatToRPY(target2base_link.transform.rotation, roll, pitch, yaw); + tf2::Quaternion quat_tf; + quat_tf.setRPY(0, 0, yaw * yaw_scale_); + geometry_msgs::Quaternion quat_msg = tf2::toMsg(quat_tf); + target_.pose.orientation = quat_msg; + } + catch (tf2::TransformException& ex) + { + ROS_WARN("%s", ex.what()); + return false; + } + } + + interface_.setGoal(target_); + return true; + } + +private: + double x_offset_, y_offset_, yaw_scale_; + std::string move_target_; + tf2_ros::Buffer& tf_buffer_; +}; + }; // namespace engineer_middleware diff --git a/engineer_middleware/include/engineer_middleware/points.h b/engineer_middleware/include/engineer_middleware/points.h index c0b69eb..d179ba8 100644 --- a/engineer_middleware/include/engineer_middleware/points.h +++ b/engineer_middleware/include/engineer_middleware/points.h @@ -57,6 +57,12 @@ class Points points_final_.push_back(p); } } + void generatePointsInLine(double center_x, double center_y, double center_z, double length, double roll, double pitch, + double yaw, double point_resolution) + { + points_final_.clear(); + std::vector points; + } sensor_msgs::PointCloud2 getPointCloud2() { sensor_msgs::PointCloud2 cloud2; diff --git a/engineer_middleware/include/engineer_middleware/step.h b/engineer_middleware/include/engineer_middleware/step.h index 253c47c..3f5024d 100644 --- a/engineer_middleware/include/engineer_middleware/step.h +++ b/engineer_middleware/include/engineer_middleware/step.h @@ -59,7 +59,7 @@ class Step if (step.hasMember("arm")) { if (step["arm"].hasMember("joints")) - arm_motion_ = new JointMotion(step["arm"], arm_group); + arm_motion_ = new JointMotion(step["arm"], arm_group, tf); else if (step["arm"].hasMember("spacial_shape")) arm_motion_ = new SpaceEeMotion(step["arm"], arm_group, tf); else @@ -98,6 +98,8 @@ class Step if (step["extend_arm"].hasMember("back")) extend_arm_back_motion_ = new ExtendMotion(step["extend_arm"], extend_arm_b_pub, false); } + if (step.hasMember("chassis_target")) + chassis_target_motion_ = new ChassisTargetMotion(step["chassis_target"], chassis_interface, tf); } bool move() { @@ -139,6 +141,8 @@ class Step success &= extend_arm_back_motion_->move(); if (extend_arm_front_motion_) success &= extend_arm_front_motion_->move(); + if (chassis_target_motion_) + success &= chassis_target_motion_->move(); return success; } void stop() @@ -149,6 +153,8 @@ class Step hand_motion_->stop(); if (chassis_motion_) chassis_motion_->stop(); + if (chassis_target_motion_) + chassis_target_motion_->stop(); } void deleteScene() @@ -176,6 +182,8 @@ class Step success &= gimbal_motion_->isFinish(); if (reversal_motion_) success &= reversal_motion_->isFinish(); + if (chassis_target_motion_) + success &= chassis_target_motion_->isFinish(); return success; } bool checkTimeout(ros::Duration period) @@ -191,6 +199,8 @@ class Step success &= chassis_motion_->checkTimeout(period); if (gimbal_motion_) success &= gimbal_motion_->checkTimeout(period); + if (chassis_target_motion_) + success &= chassis_target_motion_->checkTimeout(period); return success; } @@ -216,6 +226,7 @@ class Step PlanningScene* planning_scene_{}; moveit::planning_interface::PlanningSceneInterface planning_scene_interface_; moveit::planning_interface::MoveGroupInterface& arm_group_; + ChassisTargetMotion* chassis_target_motion_{}; }; } // namespace engineer_middleware From d14681c7184d6456f6d3f8cde110a706ff96e2bc Mon Sep 17 00:00:00 2001 From: cc0h <7921166012@qq.com> Date: Sat, 4 May 2024 01:57:40 +0800 Subject: [PATCH 20/80] fix ore lifter don't fall when an ore is in bin. --- engineer_middleware/config/steps_list.yaml | 131 +++++++++++++++++++-- 1 file changed, 118 insertions(+), 13 deletions(-) diff --git a/engineer_middleware/config/steps_list.yaml b/engineer_middleware/config/steps_list.yaml index 46a66f5..32aa9b0 100644 --- a/engineer_middleware/config/steps_list.yaml +++ b/engineer_middleware/config/steps_list.yaml @@ -329,11 +329,6 @@ common: 2.6 steps_list: - TEST: - - step: "test" - chassis_target: - <<: *TEST - EXCHANGE_POS: - step: "gimbal look front" gimbal: @@ -361,6 +356,12 @@ steps_list: ############################ HOME ########################## HOME: + - step: "close gripper" + gripper: + <<: *CLOSE_GRIPPER + - step: "pre_home" + arm: + <<: *PRE_HOME - step: "gimbal look front" gimbal: <<: *FRONT_POS @@ -371,18 +372,12 @@ steps_list: extend_arm: 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: "pre_home" - arm: - <<: *PRE_HOME - step: "home" arm: <<: *HOME @@ -398,7 +393,7 @@ steps_list: ################### SMALL_ISLAND_FROM_UP ################ ########ONE_STONE_ONCE######### - ONE_STONE_SMALL_ISLAND: + 0_TAKE_ONE_STONE_SMALL_ISLAND: - step: "GIMBAL_READY" gimbal: <<: *FRONT_POS @@ -421,7 +416,7 @@ steps_list: - step: "gimbal lifter ready" gimbal_lifter: target: *LIFTER_SMALL_ISLAND - ONE_STONE_SMALL_ISLAND0: + 0_TAKE_ONE_STONE_SMALL_ISLAND0: - step: "GET_STONE" arm: joints: [ *JOINT1_SMALL_DOWN, *JOINT2_SMALL_READY, *JOINT3_SMALL_READY_MID, *JOINT4_SMALL_READY, *JOINT5_SMALL_READY, *JOINT6_SMALL_READY, *JOINT7_SMALL_READY ] @@ -500,6 +495,108 @@ steps_list: - step: "ADD_STONE" stone_num: change: "SILVER" + 1_TAKE_ONE_STONE_SMALL_ISLAND: + - step: "GIMBAL_READY" + gimbal: + <<: *FRONT_POS + - step: "ORE_ROTATOR_READY" + ore_rotator: + target: *READY_POS + - step: "ORE_LIFTER_READY" + ore_lifter: + target: *BIN_DOWN_POS + - step: "ARM_READY" + arm: + joints: [ *JOINT1_SMALL_READY, *JOINT2_SMALL_READY, *JOINT3_SMALL_READY_MID, *JOINT4_SMALL_READY, *JOINT5_SMALL_READY, *JOINT6_SMALL_READY, *JOINT7_SMALL_READY ] + common: + <<: *NORMALLY + tolerance: + <<: *NORMAL_TOLERANCE + - step: "OPEN_GRIPPER" + gripper: + <<: *OPEN_GRIPPER + - step: "gimbal lifter ready" + gimbal_lifter: + target: *LIFTER_SMALL_ISLAND + 1_TAKE_ONE_STONE_SMALL_ISLAND0: + - step: "GET_STONE" + arm: + joints: [ *JOINT1_SMALL_DOWN, *JOINT2_SMALL_READY, *JOINT3_SMALL_READY_MID, *JOINT4_SMALL_READY, *JOINT5_SMALL_READY, *JOINT6_SMALL_READY, *JOINT7_SMALL_READY ] + common: + <<: *NORMALLY + tolerance: + <<: *NORMAL_TOLERANCE + - step: "JOINT1_UP" + arm: + joints: [ *JOINT1_SMALL_READY, "KEEP", "KEEP", "KEEP", "KEEP", "KEEP", "KEEP" ] + common: + <<: *NORMALLY + tolerance: + <<: *NORMAL_TOLERANCE + - step: "ARM_UP_STONE" + arm: + joints: [ *JOINT1_SMALL_READY, *JOINT2_SMALL_READY, *JOINT3_SMALL_READY_MID, *JOINT4_SMALL_READY, *JOINT5_SMALL_UP_STONE, *JOINT6_SMALL_READY, *JOINT7_SMALL_READY ] + common: + <<: *NORMALLY + tolerance: + <<: *NORMAL_TOLERANCE + - step: "JOINT6_BACK" + arm: + joints: [ "KEEP", "KEEP", "KEEP", "KEEP", "KEEP", *JOINT6_SMALL_BACK, "KEEP" ] + common: + <<: *NORMALLY + tolerance: + <<: *NORMAL_TOLERANCE + - step: "TURN_STONE" + arm: + joints: [ *JOINT1_SMALL_READY, *JOINT2_SMALL_READY, *JOINT3_SMALL_TURN, *JOINT4_SMALL_TURN, *JOINT5_SMALL_UP_STONE, *JOINT6_SMALL_READY, *JOINT7_SMALL_READY ] + common: + <<: *NORMALLY + tolerance: + <<: *NORMAL_TOLERANCE + - step: "READY_STORE" + arm: + joints: [ *JOINT1_SMALL_READY_STORE, *JOINT2_SMALL_STORE, *JOINT3_SMALL_TURN, *JOINT4_SMALL_TURN, *JOINT5_SMALL_UP_STONE, *JOINT6_SMALL_READY_STORE, *JOINT7_SMALL_READY_STORE ] + common: + <<: *NORMALLY + tolerance: + <<: *NORMAL_TOLERANCE + - step: "STORE" + arm: + joints: [ *JOINT1_SMALL_READY_STORE, *JOINT2_SMALL_STORE, *JOINT3_SMALL_TURN, *JOINT4_SMALL_TURN, *JOINT5_SMALL_STORE, *JOINT6_SMALL_READY_STORE, *JOINT7_SMALL_READY_STORE ] + common: + <<: *SLOWLY + tolerance: + <<: *NORMAL_TOLERANCE + # ONE_STONE_SMALL_ISLAND00: + - step: "CLOSE_GRIPPER" + gripper: + <<: *CLOSE_GRIPPER + - step: "MOVE_ARM_OUT" + arm: + joints: [ *JOINT1_SMALL_READY_STORE, *JOINT2_SMALL_READY, *JOINT3_SMALL_TURN, *JOINT4_R_POSITION, *JOINT5_MID_POSITION, *JOINT6_UP_POSITION, *JOINT7_MID_POSITION ] + common: + <<: *SLOWLY + tolerance: + <<: *NORMAL_TOLERANCE + - step: "CHASSIS_BACK" + chassis: + <<: *CHASSIS_BACK + - step: "CHASSIS_TURN" + chassis: + <<: *CHASSIS_TURN + - step: "pre_home" + arm: + <<: *PRE_HOME + - step: "home" + arm: + <<: *HOME + - step: "ORE_LIFTER_DOWN" + ore_lifter: + target: *BIN_DOWN_POS + - step: "ADD_STONE" + stone_num: + change: "SILVER" ########################## TWO_STONE_ONCE ######################## ######FIRST_STONE######## @@ -1454,6 +1551,14 @@ steps_list: tolerance: <<: *NORMAL_TOLERANCE + CHASSIS_TARGET_TEST2: + - step: "test" + chassis: + frame: "base_link" + offset: [0., 0.] + yaw_scale: 0.9 + target_frame: "arm" + CHASSIS_TARGET_TEST1: - step: "test" chassis: From 411a94600ad32169ba7657118ef4d601de63e42a Mon Sep 17 00:00:00 2001 From: cc0h <7921166012@qq.com> Date: Sat, 4 May 2024 02:00:10 +0800 Subject: [PATCH 21/80] Add arm back which just move back joint 2 and 3. --- .../include/engineer_middleware/motion.h | 28 +++++++++---------- engineer_middleware/src/middleware.cpp | 1 + 2 files changed, 15 insertions(+), 14 deletions(-) diff --git a/engineer_middleware/include/engineer_middleware/motion.h b/engineer_middleware/include/engineer_middleware/motion.h index f216dcc..85fcac2 100644 --- a/engineer_middleware/include/engineer_middleware/motion.h +++ b/engineer_middleware/include/engineer_middleware/motion.h @@ -409,6 +409,7 @@ class JointMotion : public MoveitMotionBase if (motion.hasMember("record_arm2base")) record_arm2base_ = bool(motion["record_arm2base"]); } + static geometry_msgs::TransformStamped arm2base; bool move() override { if (record_arm2base_) @@ -418,7 +419,9 @@ class JointMotion : public MoveitMotionBase arm2base.header.frame_id = "base_link"; arm2base.header.stamp = ros::Time::now(); arm2base.child_frame_id = "chassis_target"; - arm2base = tf_buffer_.lookupTransform("base_link", "joint4", ros::Time(0)); + + arm2base = tf_buffer_.lookupTransform("base_link", "link4", ros::Time(0)); + ROS_INFO_STREAM("X is: " << arm2base.transform.translation.x << "Y is: " << arm2base.transform.translation.y); } catch (tf2::TransformException& ex) { @@ -446,7 +449,6 @@ class JointMotion : public MoveitMotionBase msg_.data = interface_.plan(plan).val; return (interface_.asyncExecute(plan) == moveit::planning_interface::MoveItErrorCode::SUCCESS); } - static geometry_msgs::TransformStamped arm2base; private: bool isReachGoal() override @@ -763,12 +765,10 @@ class ChassisTargetMotion : public ChassisMotion { try { - double roll, pitch, yaw; - target_.pose.position.x = JointMotion::arm2base.transform.translation.x; - target_.pose.position.y = JointMotion::arm2base.transform.translation.y; - quatToRPY(JointMotion::arm2base.transform.rotation, roll, pitch, yaw); + target_.pose.position.x = JointMotion::arm2base.transform.translation.x + x_offset_; + target_.pose.position.y = JointMotion::arm2base.transform.translation.y + y_offset_; tf2::Quaternion quat_tf; - quat_tf.setRPY(0, 0, yaw * yaw_scale_); + quat_tf.setRPY(0, 0, 0); geometry_msgs::Quaternion quat_msg = tf2::toMsg(quat_tf); target_.pose.orientation = quat_msg; } @@ -783,11 +783,11 @@ class ChassisTargetMotion : public ChassisMotion try { double roll, pitch, yaw; - geometry_msgs::TransformStamped target2base_link; - target2base_link = tf_buffer_.lookupTransform("base_link", move_target_, ros::Time(0)); - target_.pose.position.x = target2base_link.transform.translation.x; - target_.pose.position.y = target2base_link.transform.translation.y; - quatToRPY(target2base_link.transform.rotation, roll, pitch, yaw); + geometry_msgs::TransformStamped base2target; + base2target = tf_buffer_.lookupTransform("base_link", move_target_, ros::Time(0)); + target_.pose.position.x = base2target.transform.translation.x + x_offset_; + target_.pose.position.y = base2target.transform.translation.y + y_offset_; + quatToRPY(base2target.transform.rotation, roll, pitch, yaw); tf2::Quaternion quat_tf; quat_tf.setRPY(0, 0, yaw * yaw_scale_); geometry_msgs::Quaternion quat_msg = tf2::toMsg(quat_tf); @@ -805,8 +805,8 @@ class ChassisTargetMotion : public ChassisMotion } private: - double x_offset_, y_offset_, yaw_scale_; - std::string move_target_; + double x_offset_{}, y_offset_{}, yaw_scale_{}; + std::string move_target_{}; tf2_ros::Buffer& tf_buffer_; }; diff --git a/engineer_middleware/src/middleware.cpp b/engineer_middleware/src/middleware.cpp index 3ef4368..686e9f2 100644 --- a/engineer_middleware/src/middleware.cpp +++ b/engineer_middleware/src/middleware.cpp @@ -82,5 +82,6 @@ Middleware::Middleware(ros::NodeHandle& nh) ROS_ERROR("no steps list define in yaml"); as_.start(); } +geometry_msgs::TransformStamped engineer_middleware::JointMotion::arm2base; } // namespace engineer_middleware From 9c8e59b8a187dd0412b4986871fb074ca73d4158 Mon Sep 17 00:00:00 2001 From: cc0h <7921166012@qq.com> Date: Sat, 4 May 2024 16:05:49 +0800 Subject: [PATCH 22/80] Add chassis motion to compensate arm. --- .../include/engineer_middleware/motion.h | 34 ++++++++++++++++--- 1 file changed, 29 insertions(+), 5 deletions(-) diff --git a/engineer_middleware/include/engineer_middleware/motion.h b/engineer_middleware/include/engineer_middleware/motion.h index 85fcac2..12de930 100644 --- a/engineer_middleware/include/engineer_middleware/motion.h +++ b/engineer_middleware/include/engineer_middleware/motion.h @@ -754,6 +754,10 @@ class ChassisTargetMotion : public ChassisMotion ChassisTargetMotion(XmlRpc::XmlRpcValue& motion, ChassisInterface& interface, tf2_ros::Buffer& tf_buffer) : ChassisMotion(motion, interface), tf_buffer_(tf_buffer) { + chassis_tolerance_position_ = xmlRpcGetDouble(motion, "chassis_tolerance_position", 0.01); + chassis_tolerance_angular_ = xmlRpcGetDouble(motion, "chassis_tolerance_angular", 0.01); + if (motion.hasMember("frame")) + target_.header.frame_id = std::string(motion["frame"]); x_offset_ = xmlRpcGetDouble(motion["offset"], 0); y_offset_ = xmlRpcGetDouble(motion["offset"], 1); yaw_scale_ = xmlRpcGetDouble(motion, "yaw_scale", 1); @@ -763,14 +767,21 @@ class ChassisTargetMotion : public ChassisMotion { if (move_target_ == "arm") { + ROS_INFO_STREAM("TARGET IS ARM"); try { - target_.pose.position.x = JointMotion::arm2base.transform.translation.x + x_offset_; - target_.pose.position.y = JointMotion::arm2base.transform.translation.y + y_offset_; + geometry_msgs::TransformStamped arm2base_now = tf_buffer_.lookupTransform("base_link", "link4", ros::Time(0)); + + target_.pose.position.x = + JointMotion::arm2base.transform.translation.x - arm2base_now.transform.translation.x + x_offset_; + target_.pose.position.y = + JointMotion::arm2base.transform.translation.y - arm2base_now.transform.translation.y + y_offset_; tf2::Quaternion quat_tf; quat_tf.setRPY(0, 0, 0); geometry_msgs::Quaternion quat_msg = tf2::toMsg(quat_tf); target_.pose.orientation = quat_msg; + interface_.setGoal(target_); + return true; } catch (tf2::TransformException& ex) { @@ -780,6 +791,7 @@ class ChassisTargetMotion : public ChassisMotion } else { + ROS_INFO_STREAM("TARGET IS " << move_target_); try { double roll, pitch, yaw; @@ -787,11 +799,17 @@ class ChassisTargetMotion : public ChassisMotion base2target = tf_buffer_.lookupTransform("base_link", move_target_, ros::Time(0)); target_.pose.position.x = base2target.transform.translation.x + x_offset_; target_.pose.position.y = base2target.transform.translation.y + y_offset_; + ROS_INFO_STREAM("base2target x: " << base2target.transform.translation.x); + ROS_INFO_STREAM("base2target y: " << base2target.transform.translation.y); + ROS_INFO_STREAM("target x: " << target_.pose.position.x); + ROS_INFO_STREAM("target y: " << target_.pose.position.y); quatToRPY(base2target.transform.rotation, roll, pitch, yaw); tf2::Quaternion quat_tf; quat_tf.setRPY(0, 0, yaw * yaw_scale_); geometry_msgs::Quaternion quat_msg = tf2::toMsg(quat_tf); target_.pose.orientation = quat_msg; + interface_.setGoal(target_); + return true; } catch (tf2::TransformException& ex) { @@ -799,9 +817,15 @@ class ChassisTargetMotion : public ChassisMotion return false; } } - - interface_.setGoal(target_); - return true; + } + bool isFinish() override + { + return interface_.getErrorPos() < chassis_tolerance_position_ && + interface_.getErrorYaw() < chassis_tolerance_angular_; + } + void stop() override + { + interface_.stop(); } private: From 43528482a676f9534b2e8c5613ae59dfb286c4fd Mon Sep 17 00:00:00 2001 From: cc0h <7921166012@qq.com> Date: Sat, 4 May 2024 16:07:35 +0800 Subject: [PATCH 23/80] update sim.launch. --- engineer_middleware/launch/sim.launch | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/engineer_middleware/launch/sim.launch b/engineer_middleware/launch/sim.launch index 5b7857f..394f0ff 100644 --- a/engineer_middleware/launch/sim.launch +++ b/engineer_middleware/launch/sim.launch @@ -1,9 +1,10 @@ + + + - - From 047f146856ad4f1d7a7ef7af641f41d24d38e787 Mon Sep 17 00:00:00 2001 From: cc0h <7921166012@qq.com> Date: Sun, 5 May 2024 00:02:35 +0800 Subject: [PATCH 24/80] record. --- engineer_middleware/config/steps_list.yaml | 148 +++++++++++---------- 1 file changed, 75 insertions(+), 73 deletions(-) diff --git a/engineer_middleware/config/steps_list.yaml b/engineer_middleware/config/steps_list.yaml index 32aa9b0..3cee057 100644 --- a/engineer_middleware/config/steps_list.yaml +++ b/engineer_middleware/config/steps_list.yaml @@ -76,7 +76,7 @@ common: bin_get_stone: &JOINT2_BIN_GET_STONE 0.087 back_bin_position: &JOINT2_BACK_BIN_POSITION - 0.001 + 0.05 big_ready: &JOINT2_BIG_READY 0.03 big_arm_up: &JOINT2_BIG_ARM_UP @@ -1540,6 +1540,21 @@ steps_list: ore_lifter: target: *BIN_UP_POS + 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 + #################################################### TEST ########################################################## ARM_BACK_TEST: - step: "arm back test" @@ -1550,41 +1565,41 @@ steps_list: <<: *NORMALLY tolerance: <<: *NORMAL_TOLERANCE - - CHASSIS_TARGET_TEST2: + ARM_BACK_TEST1: - step: "test" - chassis: - frame: "base_link" - offset: [0., 0.] + chassis_target: + frame: base_link + position: [ 0., 0. ] + yaw: 0. + offset: [ 0., 0. ] yaw_scale: 0.9 - target_frame: "arm" + target_frame: arm + common: + timeout: 2. + + +# - step: "test2" +# chassis: +# frame: base_link +# position: [ 0., 0. ] +# yaw: -1.40 +# chassis_tolerance_position: 0.1 +# chassis_tolerance_angular: 0.2 +# common: +# timeout: 2. + CHASSIS_TARGET_TEST1: - step: "test" - chassis: - frame: "base_link" + chassis_target: + frame: base_link offset: [ 0., 0. ] yaw_scale: 0.9 - target_frame: "exchanger" - - ########## 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 + target_frame: exchanger common: - <<: *QUICKLY + timeout: 2. + ########## EXCHANGE ############ EXCHANGE_DOWN: - step: "test new exchange" arm: @@ -1602,28 +1617,13 @@ steps_list: 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: + EXCHANGE_STRAIGHT: - step: "test new exchange" arm: frame: exchanger is_refer_planning_frame: true - rpy: [ 0., -1.5707963, 0. ] + 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 @@ -1633,8 +1633,17 @@ steps_list: max_planning_times: 1 common: <<: *NORMALLY - EXCHANGE_STRAIGHT: - - step: "test new exchange" + + AUTO_TEST: + - step: "chassis approach" + chassis_target: + frame: base_link + offset: [ 0., 0. ] + yaw_scale: 0.9 + target_frame: exchanger + common: + timeout: 2. + - step: "auto sphere exchange" arm: frame: exchanger is_refer_planning_frame: true @@ -1649,29 +1658,22 @@ 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 - - CHASSIS_TEST1: - - step: "chassis turn" - chassis: - <<: *CHASSIS_TURN - - CHASSIS_TEST2: - - step: "chassis back" - chassis: - <<: *CHASSIS_BACK + - step: "arm back" + arm: + joints: [ "KEEP", *JOINT2_BACK_POSITION, *JOINT3_HOME_POS, "KEEP", "KEEP", "KEEP", "KEEP" ] + record_arm2base: true + common: + <<: *NORMALLY + tolerance: + <<: *NORMAL_TOLERANCE + AUTO_TEST0: + - step: "chassis compensation" + chassis_target: + frame: base_link + position: [ 0., 0. ] + yaw: 0. + offset: [ 0., 0. ] + yaw_scale: 0.9 + target_frame: arm + common: + timeout: 2. From acc1fa6cd493aa1ada4faa5dd7c83a62266dd23b Mon Sep 17 00:00:00 2001 From: cc0h <7921166012@qq.com> Date: Wed, 8 May 2024 02:06:37 +0800 Subject: [PATCH 25/80] record steps list. --- engineer_middleware/config/steps_list.yaml | 327 +++++++++++++++++---- 1 file changed, 262 insertions(+), 65 deletions(-) diff --git a/engineer_middleware/config/steps_list.yaml b/engineer_middleware/config/steps_list.yaml index 3cee057..1678a00 100644 --- a/engineer_middleware/config/steps_list.yaml +++ b/engineer_middleware/config/steps_list.yaml @@ -19,9 +19,9 @@ common: normal_tolerance: &NORMAL_TOLERANCE tolerance_joints: [ 0.01, 0.010, 0.015, 0.3, 0.2, 0.15, 0.07 ] bigger_tolerance: &BIGGER_TOLERANCE - tolerance_joints: [ 0.005, 0.010, 0.01, 0.3, 0.2, 0.2 ] + tolerance_joints: [ 0.015, 0.015, 0.02, 0.3, 0.2, 0.2, 0.2 ] max_tolerance: &MAX_TOLERANCE - tolerance_joints: [ 0.03, 0.03, 0.03, 0.3, 0.3, 0.3 ] + tolerance_joints: [ 0.03, 0.03, 0.03, 0.3, 0.3, 0.3 , 0.3] joint1: mechanical: @@ -49,6 +49,8 @@ common: 0.320 big_ready_store: &JOINT1_BIG_READY_STORE 0.133 + bin_get_stone: &JOINT1_BIN_GET_STONE + 0.1668 home: zero_stone_position: &JOINT1_ZERO_STONE_POSITION 0.01 @@ -74,9 +76,11 @@ common: small_store: &JOINT2_SMALL_STORE 0.044 bin_get_stone: &JOINT2_BIN_GET_STONE - 0.087 + 0.13 back_bin_position: &JOINT2_BACK_BIN_POSITION 0.05 + avoid_collision: &JOINT2_AVOID_COLLISION + 0.1 big_ready: &JOINT2_BIG_READY 0.03 big_arm_up: &JOINT2_BIG_ARM_UP @@ -104,6 +108,8 @@ common: 0.257 bin_get_stone: &JOINT3_BIN_GET_STONE 0.252 + bin_get_lv4_stone: &JOINT3_BIN_GET_LV4_STONE + 0.31 big_ready: &JOINT3_BIG_READY 0.071 get_big_mid: &JOINT3_GET_BIG_MID @@ -213,16 +219,20 @@ common: 1.594 gimbal: - side_pos: &SIDE_POS + right_pos: &RIGHT_POS frame: gimbal_lifter position: [ 0.001 , -3., 0. ] + left_pos: &LEFT_POS + frame: gimbal_lifter + position: [ 0.001, 3., 0. ] front_pos: &FRONT_POS frame: gimbal_lifter position: [ 2., 0.001, 0. ] - small_island_pos: &SMALL_ISLAND_POS + small_island_pos: &BIG_ISLAND_POS frame: gimbal_lifter position: [ 1., -1., 0.] + gimbal_lifter: button_pos: &BUTTON_POS 0.05 @@ -248,9 +258,9 @@ common: pre_home: &PRE_HOME joints: [ *JOINT1_ZERO_STONE_POSITION, *JOINT2_BACK_POSITION, *JOINT3_HOME_POS, *JOINT4_PRE_HOME, *JOINT5_MID_POSITION, *JOINT6_UP_POSITION, *JOINT7_MID_POSITION ] common: - <<: *NORMALLY + <<: *QUICKLY tolerance: - <<: *SMALL_TOLERANCE + <<: *NORMAL_TOLERANCE home: &HOME joints: [ *JOINT1_ZERO_STONE_POSITION, *JOINT2_BACK_POSITION, *JOINT3_HOME_POS, *JOINT4_R_POSITION, *JOINT5_MID_POSITION, *JOINT6_UP_POSITION, *JOINT7_MID_POSITION ] @@ -314,6 +324,8 @@ common: ore_lifter: bin_up_pos: &BIN_UP_POS 0.2 + small_up_pos: &BIN_SMALL_UP_POS + 0.03 bin_mid_pos: &BIN_MID_POS 0.1 bin_down_pos: &BIN_DOWN_POS @@ -344,14 +356,6 @@ steps_list: gimbal_lifter: target: *TALLEST_POS - GIMBAL_FRONT: - - step: "gimbal look front" - gimbal: - <<: *FRONT_POS - GIMBAL_LEFT: - - step: "gimbal look left" - gimbal: - <<: *SIDE_POS ############ MID ############# ############################ HOME ########################## @@ -407,7 +411,7 @@ steps_list: arm: joints: [ *JOINT1_SMALL_READY, *JOINT2_SMALL_READY, *JOINT3_SMALL_READY_MID, *JOINT4_SMALL_READY, *JOINT5_SMALL_READY, *JOINT6_SMALL_READY, *JOINT7_SMALL_READY ] common: - <<: *NORMALLY + <<: *QUICKLY tolerance: <<: *NORMAL_TOLERANCE - step: "OPEN_GRIPPER" @@ -442,28 +446,28 @@ steps_list: arm: joints: [ "KEEP", "KEEP", "KEEP", "KEEP", "KEEP", *JOINT6_SMALL_BACK, "KEEP" ] common: - <<: *NORMALLY + <<: *QUICKLY tolerance: <<: *NORMAL_TOLERANCE - step: "TURN_STONE" arm: joints: [ *JOINT1_SMALL_READY, *JOINT2_SMALL_READY, *JOINT3_SMALL_TURN, *JOINT4_SMALL_TURN, *JOINT5_SMALL_UP_STONE, *JOINT6_SMALL_READY, *JOINT7_SMALL_READY ] common: - <<: *NORMALLY + <<: *QUICKLY tolerance: <<: *NORMAL_TOLERANCE - step: "READY_STORE" arm: joints: [ *JOINT1_SMALL_READY_STORE, *JOINT2_SMALL_STORE, *JOINT3_SMALL_TURN, *JOINT4_SMALL_TURN, *JOINT5_SMALL_UP_STONE, *JOINT6_SMALL_READY_STORE, *JOINT7_SMALL_READY_STORE ] common: - <<: *NORMALLY + <<: *QUICKLY tolerance: <<: *NORMAL_TOLERANCE - step: "STORE" arm: joints: [ *JOINT1_SMALL_READY_STORE, *JOINT2_SMALL_STORE, *JOINT3_SMALL_TURN, *JOINT4_SMALL_TURN, *JOINT5_SMALL_STORE, *JOINT6_SMALL_READY_STORE, *JOINT7_SMALL_READY_STORE ] common: - <<: *SLOWLY + <<: *NORMALLY tolerance: <<: *NORMAL_TOLERANCE # ONE_STONE_SMALL_ISLAND00: @@ -474,7 +478,7 @@ steps_list: arm: joints: [ *JOINT1_SMALL_READY_STORE, *JOINT2_SMALL_READY, *JOINT3_SMALL_TURN, *JOINT4_R_POSITION, *JOINT5_MID_POSITION, *JOINT6_UP_POSITION, *JOINT7_MID_POSITION ] common: - <<: *SLOWLY + <<: *QUICKLY tolerance: <<: *NORMAL_TOLERANCE - step: "CHASSIS_BACK" @@ -509,7 +513,7 @@ steps_list: arm: joints: [ *JOINT1_SMALL_READY, *JOINT2_SMALL_READY, *JOINT3_SMALL_READY_MID, *JOINT4_SMALL_READY, *JOINT5_SMALL_READY, *JOINT6_SMALL_READY, *JOINT7_SMALL_READY ] common: - <<: *NORMALLY + <<: *QUICKLY tolerance: <<: *NORMAL_TOLERANCE - step: "OPEN_GRIPPER" @@ -544,28 +548,28 @@ steps_list: arm: joints: [ "KEEP", "KEEP", "KEEP", "KEEP", "KEEP", *JOINT6_SMALL_BACK, "KEEP" ] common: - <<: *NORMALLY + <<: *QUICKLY tolerance: <<: *NORMAL_TOLERANCE - step: "TURN_STONE" arm: joints: [ *JOINT1_SMALL_READY, *JOINT2_SMALL_READY, *JOINT3_SMALL_TURN, *JOINT4_SMALL_TURN, *JOINT5_SMALL_UP_STONE, *JOINT6_SMALL_READY, *JOINT7_SMALL_READY ] common: - <<: *NORMALLY + <<: *QUICKLY tolerance: <<: *NORMAL_TOLERANCE - step: "READY_STORE" arm: joints: [ *JOINT1_SMALL_READY_STORE, *JOINT2_SMALL_STORE, *JOINT3_SMALL_TURN, *JOINT4_SMALL_TURN, *JOINT5_SMALL_UP_STONE, *JOINT6_SMALL_READY_STORE, *JOINT7_SMALL_READY_STORE ] common: - <<: *NORMALLY + <<: *QUICKLY tolerance: <<: *NORMAL_TOLERANCE - step: "STORE" arm: joints: [ *JOINT1_SMALL_READY_STORE, *JOINT2_SMALL_STORE, *JOINT3_SMALL_TURN, *JOINT4_SMALL_TURN, *JOINT5_SMALL_STORE, *JOINT6_SMALL_READY_STORE, *JOINT7_SMALL_READY_STORE ] common: - <<: *SLOWLY + <<: *NORMALLY tolerance: <<: *NORMAL_TOLERANCE # ONE_STONE_SMALL_ISLAND00: @@ -576,7 +580,7 @@ steps_list: arm: joints: [ *JOINT1_SMALL_READY_STORE, *JOINT2_SMALL_READY, *JOINT3_SMALL_TURN, *JOINT4_R_POSITION, *JOINT5_MID_POSITION, *JOINT6_UP_POSITION, *JOINT7_MID_POSITION ] common: - <<: *SLOWLY + <<: *QUICKLY tolerance: <<: *NORMAL_TOLERANCE - step: "CHASSIS_BACK" @@ -614,7 +618,7 @@ steps_list: arm: joints: [ *JOINT1_SMALL_READY, *JOINT2_SMALL_READY, *JOINT3_SMALL_READY_LEFT, *JOINT4_SMALL_READY, *JOINT5_SMALL_READY, *JOINT6_SMALL_READY, *JOINT7_SMALL_READY ] common: - <<: *NORMALLY + <<: *QUICKLY tolerance: <<: *NORMAL_TOLERANCE - step: "OPEN_GRIPPER" @@ -642,35 +646,35 @@ steps_list: arm: joints: [ *JOINT1_SMALL_READY, *JOINT2_SMALL_READY, *JOINT3_SMALL_READY_LEFT, *JOINT4_SMALL_READY, *JOINT5_SMALL_UP_STONE, *JOINT6_SMALL_READY, *JOINT7_SMALL_READY ] common: - <<: *NORMALLY + <<: *QUICKLY tolerance: <<: *NORMAL_TOLERANCE - step: "JOINT6_BACK" arm: joints: [ "KEEP", "KEEP", "KEEP", "KEEP", "KEEP", *JOINT6_SMALL_BACK, "KEEP" ] common: - <<: *NORMALLY + <<: *QUICKLY tolerance: <<: *NORMAL_TOLERANCE - step: "TURN_STONE" arm: joints: [ *JOINT1_SMALL_READY, *JOINT2_SMALL_READY, *JOINT3_SMALL_TURN, *JOINT4_SMALL_TURN, *JOINT5_SMALL_UP_STONE, *JOINT6_SMALL_READY, *JOINT7_SMALL_READY ] common: - <<: *NORMALLY + <<: *QUICKLY tolerance: <<: *NORMAL_TOLERANCE - step: "READY_STORE" arm: joints: [ *JOINT1_SMALL_READY_STORE, *JOINT2_SMALL_STORE, *JOINT3_SMALL_TURN, *JOINT4_SMALL_TURN, *JOINT5_SMALL_UP_STONE, *JOINT6_SMALL_READY_STORE, *JOINT7_SMALL_READY_STORE ] common: - <<: *NORMALLY + <<: *QUICKLY tolerance: <<: *NORMAL_TOLERANCE - step: "STORE" arm: joints: [ *JOINT1_SMALL_READY_STORE, *JOINT2_SMALL_STORE, *JOINT3_SMALL_TURN, *JOINT4_SMALL_TURN, *JOINT5_SMALL_STORE, *JOINT6_SMALL_READY_STORE, *JOINT7_SMALL_READY_STORE ] common: - <<: *SLOWLY + <<: *NORMALLY tolerance: <<: *NORMAL_TOLERANCE - step: "CLOSE_GRIPPER" @@ -680,7 +684,7 @@ steps_list: arm: joints: [ *JOINT1_SMALL_READY_STORE, *JOINT2_SMALL_READY, *JOINT3_SMALL_TURN, *JOINT4_R_POSITION, *JOINT5_MID_POSITION, *JOINT6_UP_POSITION, *JOINT7_MID_POSITION ] common: - <<: *SLOWLY + <<: *QUICKLY tolerance: <<: *NORMAL_TOLERANCE - step: "ORE_LIFTER_DOWN" @@ -705,7 +709,7 @@ steps_list: arm: joints: [ *JOINT1_SMALL_READY, *JOINT2_SMALL_READY, *JOINT3_SMALL_READY_MID, *JOINT4_SMALL_READY, *JOINT5_SMALL_READY, *JOINT6_SMALL_READY, *JOINT7_SMALL_READY ] common: - <<: *NORMALLY + <<: *QUICKLY tolerance: <<: *NORMAL_TOLERANCE - step: "OPEN_GRIPPER" @@ -730,35 +734,35 @@ steps_list: arm: joints: [ *JOINT1_SMALL_READY, *JOINT2_SMALL_READY, *JOINT3_SMALL_READY_MID, *JOINT4_SMALL_READY, *JOINT5_SMALL_UP_STONE, *JOINT6_SMALL_READY, *JOINT7_SMALL_READY ] common: - <<: *NORMALLY + <<: *QUICKLY tolerance: <<: *NORMAL_TOLERANCE - step: "JOINT6_BACK" arm: joints: [ "KEEP", "KEEP", "KEEP", "KEEP", "KEEP", *JOINT6_SMALL_BACK, "KEEP" ] common: - <<: *NORMALLY + <<: *QUICKLY tolerance: <<: *NORMAL_TOLERANCE - step: "TURN_STONE" arm: joints: [ *JOINT1_SMALL_READY, *JOINT2_SMALL_READY, *JOINT3_SMALL_TURN, *JOINT4_SMALL_TURN, *JOINT5_SMALL_UP_STONE, *JOINT6_SMALL_READY, *JOINT7_SMALL_READY ] common: - <<: *NORMALLY + <<: *QUICKLY tolerance: <<: *NORMAL_TOLERANCE - step: "READY_STORE" arm: joints: [ *JOINT1_SMALL_READY_STORE, *JOINT2_SMALL_STORE, *JOINT3_SMALL_TURN, *JOINT4_SMALL_TURN, *JOINT5_SMALL_UP_STONE, *JOINT6_SMALL_READY_STORE, *JOINT7_SMALL_READY_STORE ] common: - <<: *NORMALLY + <<: *QUICKLY tolerance: <<: *NORMAL_TOLERANCE - step: "STORE" arm: joints: [ *JOINT1_SMALL_READY_STORE, *JOINT2_SMALL_STORE, *JOINT3_SMALL_TURN, *JOINT4_SMALL_TURN, *JOINT5_SMALL_STORE, *JOINT6_SMALL_READY_STORE, *JOINT7_SMALL_READY_STORE ] common: - <<: *SLOWLY + <<: *NORMALLY tolerance: <<: *NORMAL_TOLERANCE - step: "CLOSE_GRIPPER" @@ -768,7 +772,7 @@ steps_list: arm: joints: [ *JOINT1_SMALL_READY_STORE, *JOINT2_SMALL_READY, *JOINT3_SMALL_TURN, *JOINT4_R_POSITION, *JOINT5_MID_POSITION, *JOINT6_UP_POSITION, *JOINT7_MID_POSITION ] common: - <<: *SLOWLY + <<: *QUICKLY tolerance: <<: *NORMAL_TOLERANCE - step: "CHASSIS_BACK" @@ -1046,6 +1050,137 @@ steps_list: change: "SILVER" ######GET STONE FROM BIN######### + + GET_DOWN_STONE_BIN: + - step: "GIMBAL_READY" + gimbal: + <<: *FRONT_POS + - step: "ORE_ROTATOR_READY" + ore_rotator: + target: *EXCHANGE_POS + - step: "ORE_LIFTER_UP" + ore_lifter: + target: *BIN_UP_POS + - step: "JOINT2 move out" + arm: + joints: [ "KEEP", *JOINT2_AVOID_COLLISION, "KEEP", "KEEP", "KEEP", "KEEP", "KEEP" ] + common: + <<: *NORMALLY + tolerance: + <<: *NORMAL_TOLERANCE + - step: "READY_TO_GET" + arm: + joints: [ *JOINT1_BIN_GET_STONE, *JOINT2_BIN_GET_STONE, *JOINT3_BIN_GET_LV4_STONE, *JOINT4_R_POSITION, *JOINT5_BIN_GET_STONE, *JOINT6_BIN_GET_STONE, *JOINT7_MID_POSITION ] + common: + <<: *QUICKLY + tolerance: + <<: *NORMAL_TOLERANCE + - step: "OPEN_GRIPPER" + gripper: + <<: *OPEN_GRIPPER + - step: "JOINT2_BACK_GET_STONE" + arm: + joints: [ "KEEP", *JOINT2_BACK_BIN_POSITION, "KEEP", "KEEP", "KEEP", "KEEP", "KEEP" ] + common: + <<: *NORMALLY + tolerance: + <<: *NORMAL_TOLERANCE + - step: "JOINT1_UP" + arm: + joints: [ *JOINT1_UP_POS, "KEEP", "KEEP", "KEEP", "KEEP", "KEEP", "KEEP" ] + common: + <<: *QUICKLY + tolerance: + <<: *NORMAL_TOLERANCE + - step: "JOINT5_LIFT_STONE" + arm: + joints: [ "KEEP", "KEEP", "KEEP", "KEEP", *JOINT5_MID_POSITION, "KEEP", "KEEP" ] + common: + <<: *QUICKLY + tolerance: + <<: *NORMAL_TOLERANCE + - step: "exchange pos" + arm: + joints: [ *JOINT1_DOWN_POSITION, *JOINT2_EXCHANGE_POSITION, *JOINT3_MID_POSITION, *JOINT4_MID_POSITION, *JOINT5_MID_POSITION, *JOINT6_UP_POSITION, *JOINT7_MID_POSITION ] + common: + <<: *QUICKLY + tolerance: + <<: *NORMAL_TOLERANCE + - step: "ORE_LIFTER_DOWN" + ore_lifter: + target: *BIN_DOWN_POS + - step: "gimbal lifter tallest" + gimbal_lifter: + target: *TALLEST_POS + - 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 + - step: "roe lifter ready" + ore_lifter: + target: *BIN_SMALL_UP_POS + - step: "JOINT2 move out" + arm: + joints: [ "KEEP", *JOINT2_AVOID_COLLISION, "KEEP", "KEEP", "KEEP", "KEEP", "KEEP" ] + common: + <<: *NORMALLY + tolerance: + <<: *NORMAL_TOLERANCE + - step: "READY_TO_GET" + arm: + joints: [ *JOINT1_BIN_GET_STONE, *JOINT2_BIN_GET_STONE, *JOINT3_BIN_GET_LV4_STONE, *JOINT4_R_POSITION, *JOINT5_BIN_GET_STONE, *JOINT6_BIN_GET_STONE, *JOINT7_MID_POSITION ] + common: + <<: *QUICKLY + tolerance: + <<: *NORMAL_TOLERANCE + - step: "OPEN_GRIPPER" + gripper: + <<: *OPEN_GRIPPER + - step: "JOINT2_BACK_GET_STONE" + arm: + joints: [ "KEEP", *JOINT2_BACK_BIN_POSITION, "KEEP", "KEEP", "KEEP", "KEEP", "KEEP" ] + common: + <<: *NORMALLY + tolerance: + <<: *NORMAL_TOLERANCE + - step: "JOINT1_UP" + arm: + joints: [ *JOINT1_UP_POS, "KEEP", "KEEP", "KEEP", "KEEP", "KEEP", "KEEP" ] + common: + <<: *QUICKLY + tolerance: + <<: *NORMAL_TOLERANCE + - step: "JOINT5_LIFT_STONE" + arm: + joints: [ "KEEP", "KEEP", "KEEP", "KEEP", *JOINT5_MID_POSITION, "KEEP", "KEEP" ] + common: + <<: *QUICKLY + tolerance: + <<: *NORMAL_TOLERANCE + - step: "exchange pos" + arm: + joints: [ *JOINT1_DOWN_POSITION, *JOINT2_EXCHANGE_POSITION, *JOINT3_MID_POSITION, *JOINT4_MID_POSITION, *JOINT5_MID_POSITION, *JOINT6_UP_POSITION, *JOINT7_MID_POSITION ] + common: + <<: *QUICKLY + tolerance: + <<: *NORMAL_TOLERANCE + - step: "ORE_LIFTER_DOWN" + ore_lifter: + target: *BIN_DOWN_POS + - step: "gimbal lifter tallest" + gimbal_lifter: + target: *TALLEST_POS + - step: "remove stone" + stone_num: + change: "-1" + GET_DOWN_STONE_LEFT: - step: "GIMBAL_READY" gimbal: @@ -1056,11 +1191,18 @@ steps_list: - step: "ORE_LIFTER_UP" ore_lifter: target: *BIN_GET_DOWN + - step: "JOINT2 move out" + arm: + joints: [ "KEEP", *JOINT2_AVOID_COLLISION, "KEEP", "KEEP", "KEEP", "KEEP", "KEEP" ] + common: + <<: *NORMALLY + tolerance: + <<: *NORMAL_TOLERANCE - step: "READY_TO_GET" arm: joints: [ *JOINT1_BIN_GET_STONE_LEFT, *JOINT2_BIN_GET_STONE, *JOINT3_BIN_GET_STONE, *JOINT4_R_POSITION, *JOINT5_BIN_GET_STONE, *JOINT6_BIN_GET_STONE, *JOINT7_BIN_GET_LEFT ] common: - <<: *NORMALLY + <<: *QUICKLY tolerance: <<: *NORMAL_TOLERANCE - step: "OPEN_GRIPPER" @@ -1070,7 +1212,7 @@ steps_list: arm: joints: [ "KEEP", *JOINT2_BACK_BIN_POSITION, "KEEP", "KEEP", "KEEP", "KEEP", "KEEP" ] common: - <<: *SLOWLY + <<: *NORMALLY tolerance: <<: *NORMAL_TOLERANCE - step: "JOINT1_UP" @@ -1114,11 +1256,18 @@ steps_list: - step: "ORE_LIFTER_UP" ore_lifter: target: *BIN_GET_DOWN + - step: "JOINT2 move out" + arm: + joints: [ "KEEP", *JOINT2_AVOID_COLLISION, "KEEP", "KEEP", "KEEP", "KEEP", "KEEP" ] + common: + <<: *QUICKLY + tolerance: + <<: *NORMAL_TOLERANCE - step: "READY_TO_GET" arm: joints: [ *JOINT1_BIN_GET_STONE_RIGHT, *JOINT2_BIN_GET_STONE, *JOINT3_BIN_GET_STONE, *JOINT4_R_POSITION, *JOINT5_BIN_GET_STONE, *JOINT6_BIN_GET_STONE, *JOINT7_BIN_GET_RIGHT ] common: - <<: *NORMALLY + <<: *QUICKLY tolerance: <<: *NORMAL_TOLERANCE - step: "OPEN_GRIPPER" @@ -1128,7 +1277,7 @@ steps_list: arm: joints: [ "KEEP", *JOINT2_BACK_BIN_POSITION, "KEEP", "KEEP", "KEEP", "KEEP", "KEEP" ] common: - <<: *SLOWLY + <<: *NORMALLY tolerance: <<: *NORMAL_TOLERANCE - step: "JOINT1_UP" @@ -1172,11 +1321,18 @@ steps_list: - step: "ORE_LIFTER_READY" ore_lifter: target: *BIN_GET_UP + - step: "JOINT2 move out" + arm: + joints: [ "KEEP", *JOINT2_AVOID_COLLISION, "KEEP", "KEEP", "KEEP", "KEEP", "KEEP" ] + common: + <<: *NORMALLY + tolerance: + <<: *NORMAL_TOLERANCE - step: "READY_TO_GET" arm: joints: [ *JOINT1_BIN_GET_STONE_LEFT, *JOINT2_BIN_GET_STONE, *JOINT3_BIN_GET_STONE, *JOINT4_R_POSITION, *JOINT5_BIN_GET_STONE, *JOINT6_BIN_GET_STONE, *JOINT7_BIN_GET_LEFT ] common: - <<: *NORMALLY + <<: *QUICKLY tolerance: <<: *NORMAL_TOLERANCE - step: "OPEN_GRIPPER" @@ -1186,7 +1342,7 @@ steps_list: arm: joints: [ "KEEP", *JOINT2_BACK_BIN_POSITION, "KEEP", "KEEP", "KEEP", "KEEP", "KEEP" ] common: - <<: *SLOWLY + <<: *NORMALLY tolerance: <<: *NORMAL_TOLERANCE - step: "JOINT1_UP" @@ -1230,11 +1386,18 @@ steps_list: - step: "ORE_LIFTER_READY" ore_lifter: target: *BIN_GET_UP + - step: "JOINT2 move out" + arm: + joints: [ "KEEP", *JOINT2_AVOID_COLLISION, "KEEP", "KEEP", "KEEP", "KEEP", "KEEP" ] + common: + <<: *NORMALLY + tolerance: + <<: *NORMAL_TOLERANCE - step: "READY_TO_GET" arm: joints: [ *JOINT1_BIN_GET_STONE_RIGHT, *JOINT2_BIN_GET_STONE, *JOINT3_BIN_GET_STONE, *JOINT4_R_POSITION, *JOINT5_BIN_GET_STONE, *JOINT6_BIN_GET_STONE, *JOINT7_BIN_GET_RIGHT ] common: - <<: *NORMALLY + <<: *QUICKLY tolerance: <<: *NORMAL_TOLERANCE - step: "OPEN_GRIPPER" @@ -1244,7 +1407,7 @@ steps_list: arm: joints: [ "KEEP", *JOINT2_BACK_BIN_POSITION, "KEEP", "KEEP", "KEEP", "KEEP", "KEEP" ] common: - <<: *SLOWLY + <<: *NORMALLY tolerance: <<: *NORMAL_TOLERANCE - step: "JOINT1_UP" @@ -1283,7 +1446,7 @@ steps_list: MID_BIG_ISLAND: - step: "gimbal ready" gimbal: - <<: *SMALL_ISLAND_POS + <<: *BIG_ISLAND_POS - step: "open ex arm" extend_arm: front: *EX_OPEN @@ -1292,7 +1455,7 @@ steps_list: 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 + <<: *QUICKLY tolerance: <<: *NORMAL_TOLERANCE - step: "ORE_ROTATOR_READY" @@ -1312,7 +1475,7 @@ steps_list: 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 + <<: *NORMALLY tolerance: <<: *NORMAL_TOLERANCE - step: "ADJUST_STONE" @@ -1336,28 +1499,28 @@ steps_list: arm: joints: [ *JOINT1_BIG_UP_POS, "KEEP", "KEEP", "KEEP", "KEEP", "KEEP", "KEEP" ] common: - <<: *SLOWLY + <<: *NORMALLY tolerance: <<: *NORMAL_TOLERANCE - step: "ARM_UP_STONE" arm: joints: [ *JOINT1_BIG_UP_POS, *JOINT2_BIG_ARM_UP, *JOINT3_BIG_ARM_UP, *JOINT4_BIG_STORE, *JOINT5_BIG_READY, *JOINT6_BIG_ARM_UP, *JOINT7_BIG_POS ] common: - <<: *SLOWLY + <<: *NORMALLY tolerance: <<: *NORMAL_TOLERANCE - step: "READY_TO_STORE" arm: joints: [ *JOINT1_BIG_READY_STORE, *JOINT2_BIG_READY_STORE, *JOINT3_BIG_ARM_UP, *JOINT4_BIG_STORE, *JOINT5_BIG_READY_STORE, *JOINT6_BIG_READY_STORE, *JOINT7_BIG_POS ] common: - <<: *SLOWLY + <<: *NORMALLY tolerance: <<: *NORMAL_TOLERANCE - step: "STORE_STONE" arm: joints: [ *JOINT1_BIG_READY_STORE, *JOINT2_BIG_READY_STORE, *JOINT3_BIG_ARM_UP, *JOINT4_BIG_STORE, *JOINT5_BIG_STORE, *JOINT6_BIG_READY_STORE, *JOINT7_BIG_POS ] common: - <<: *SLOWLY + <<: *NORMALLY tolerance: <<: *NORMAL_TOLERANCE # MID_BIG_ISLAND00: @@ -1368,7 +1531,7 @@ steps_list: arm: joints: [ *JOINT1_DOWN_POSITION, *JOINT2_BACK_POSITION, "KEEP", *JOINT4_R_POSITION, *JOINT5_MID_POSITION, *JOINT6_UP_POSITION, *JOINT7_MID_POSITION ] common: - <<: *NORMALLY + <<: *QUICKLY tolerance: <<: *NORMAL_TOLERANCE - step: "pre_home" @@ -1398,7 +1561,7 @@ steps_list: SIDE_BIG_ISLAND: - step: "gimbal ready" gimbal: - <<: *SMALL_ISLAND_POS + <<: *BIG_ISLAND_POS - step: "open ex arm" extend_arm: front: *EX_OPEN @@ -1407,7 +1570,7 @@ steps_list: 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 + <<: *QUICKLY tolerance: <<: *NORMAL_TOLERANCE - step: "ORE_ROTATOR_READY" @@ -1428,7 +1591,7 @@ steps_list: 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 + <<: *NORMALLY tolerance: <<: *NORMAL_TOLERANCE - step: "ADJUST_STONE" @@ -1452,28 +1615,28 @@ steps_list: arm: joints: [ *JOINT1_BIG_UP_POS, "KEEP", "KEEP", "KEEP", "KEEP", "KEEP", "KEEP" ] common: - <<: *SLOWLY + <<: *NORMALLY tolerance: <<: *NORMAL_TOLERANCE - step: "ARM_UP_STONE" arm: joints: [ *JOINT1_BIG_UP_POS, *JOINT2_BIG_ARM_UP, *JOINT3_BIG_ARM_UP, *JOINT4_BIG_STORE, *JOINT5_BIG_READY, *JOINT6_BIG_ARM_UP, *JOINT7_BIG_POS ] common: - <<: *SLOWLY + <<: *NORMALLY tolerance: <<: *NORMAL_TOLERANCE - step: "READY_TO_STORE" arm: joints: [ *JOINT1_BIG_READY_STORE, *JOINT2_BIG_READY_STORE, *JOINT3_BIG_ARM_UP, *JOINT4_BIG_STORE, *JOINT5_BIG_READY_STORE, *JOINT6_BIG_READY_STORE, *JOINT7_BIG_POS ] common: - <<: *SLOWLY + <<: *NORMALLY tolerance: <<: *NORMAL_TOLERANCE - step: "STORE_STONE" arm: joints: [ *JOINT1_BIG_READY_STORE, *JOINT2_BIG_READY_STORE, *JOINT3_BIG_ARM_UP, *JOINT4_BIG_STORE, *JOINT5_BIG_STORE, *JOINT6_BIG_READY_STORE, *JOINT7_BIG_POS ] common: - <<: *SLOWLY + <<: *NORMALLY tolerance: <<: *NORMAL_TOLERANCE - step: "CLOSE_GRIPPER" @@ -1483,7 +1646,7 @@ steps_list: arm: joints: [ *JOINT1_DOWN_POSITION, *JOINT2_BACK_POSITION, "KEEP", *JOINT4_R_POSITION, *JOINT5_MID_POSITION, *JOINT6_UP_POSITION, *JOINT7_MID_POSITION ] common: - <<: *NORMALLY + <<: *QUICKLY tolerance: <<: *NORMAL_TOLERANCE - step: "pre_home" @@ -1508,6 +1671,11 @@ steps_list: - step: "gimbal front" gimbal: <<: *FRONT_POS + + ################## STORE ################# + + + ##########GIMBAL_LIFTER########## ZERO_STONE_GIMBAL_LIFTER: - step: "gimbal lifter zero stone pos" @@ -1540,6 +1708,8 @@ steps_list: ore_lifter: target: *BIN_UP_POS + ################ GIMBAL LIFTER ############### + GIMBAL_DOWN: - step: "gimbal lowest" gimbal_lifter: @@ -1555,6 +1725,33 @@ steps_list: gimbal_lifter: target: *TALLEST_POS + ############## ORE ROTATOR ############### + ORE_ROTATOR_INIT: + - step: "ore rotator init pos" + ore_rotator: + target: *READY_POS + + ORE_ROTATOR_EXCHANGE: + - step: "ore rotator exchange pos" + ore_rotator: + target: *EXCHANGE_POS + + ########### GIMBAL ########## + + GIMBAL_FRONT: + - step: "gimbal look front" + gimbal: + <<: *FRONT_POS + GIMBAL_LEFT: + - step: "gimbal look left" + gimbal: + <<: *LEFT_POS + GIMBAL_RIGHT: + - step: "gimbal look right" + gimbal: + <<: *RIGHT_POS + + #################################################### TEST ########################################################## ARM_BACK_TEST: - step: "arm back test" From 3c829836ec246cb8c159719b91056835b189c8ab Mon Sep 17 00:00:00 2001 From: cc0h <7921166012@qq.com> Date: Wed, 8 May 2024 23:57:17 +0800 Subject: [PATCH 26/80] record. --- engineer_middleware/config/steps_list.yaml | 29 ++++++++++++---------- 1 file changed, 16 insertions(+), 13 deletions(-) diff --git a/engineer_middleware/config/steps_list.yaml b/engineer_middleware/config/steps_list.yaml index 1678a00..b42d2a0 100644 --- a/engineer_middleware/config/steps_list.yaml +++ b/engineer_middleware/config/steps_list.yaml @@ -51,6 +51,8 @@ common: 0.133 bin_get_stone: &JOINT1_BIN_GET_STONE 0.1668 + exchange_pos: &JOINT1_EXCHANGE_POS + 0.1 home: zero_stone_position: &JOINT1_ZERO_STONE_POSITION 0.01 @@ -258,7 +260,7 @@ common: pre_home: &PRE_HOME joints: [ *JOINT1_ZERO_STONE_POSITION, *JOINT2_BACK_POSITION, *JOINT3_HOME_POS, *JOINT4_PRE_HOME, *JOINT5_MID_POSITION, *JOINT6_UP_POSITION, *JOINT7_MID_POSITION ] common: - <<: *QUICKLY + <<: *NORMALLY tolerance: <<: *NORMAL_TOLERANCE @@ -334,6 +336,7 @@ common: 0.033 bin_get_down: &BIN_GET_DOWN 0.225 + extend_arm: close: &EX_CLOSE 0.0 @@ -347,7 +350,7 @@ steps_list: <<: *FRONT_POS - step: "move arm" arm: - joints: [*JOINT1_DOWN_POSITION, *JOINT2_EXCHANGE_POSITION, *JOINT3_MID_POSITION, *JOINT4_MID_POSITION, *JOINT5_MID_POSITION, *JOINT6_UP_POSITION, *JOINT7_MID_POSITION] + joints: [*JOINT1_EXCHANGE_POS, *JOINT2_EXCHANGE_POSITION, *JOINT3_MID_POSITION, *JOINT4_MID_POSITION, *JOINT5_MID_POSITION, *JOINT6_UP_POSITION, *JOINT7_MID_POSITION] common: <<: *NORMALLY tolerance: @@ -478,7 +481,7 @@ steps_list: arm: joints: [ *JOINT1_SMALL_READY_STORE, *JOINT2_SMALL_READY, *JOINT3_SMALL_TURN, *JOINT4_R_POSITION, *JOINT5_MID_POSITION, *JOINT6_UP_POSITION, *JOINT7_MID_POSITION ] common: - <<: *QUICKLY + <<: *NORMALLY tolerance: <<: *NORMAL_TOLERANCE - step: "CHASSIS_BACK" @@ -580,7 +583,7 @@ steps_list: arm: joints: [ *JOINT1_SMALL_READY_STORE, *JOINT2_SMALL_READY, *JOINT3_SMALL_TURN, *JOINT4_R_POSITION, *JOINT5_MID_POSITION, *JOINT6_UP_POSITION, *JOINT7_MID_POSITION ] common: - <<: *QUICKLY + <<: *NORMALLY tolerance: <<: *NORMAL_TOLERANCE - step: "CHASSIS_BACK" @@ -684,7 +687,7 @@ steps_list: arm: joints: [ *JOINT1_SMALL_READY_STORE, *JOINT2_SMALL_READY, *JOINT3_SMALL_TURN, *JOINT4_R_POSITION, *JOINT5_MID_POSITION, *JOINT6_UP_POSITION, *JOINT7_MID_POSITION ] common: - <<: *QUICKLY + <<: *NORMALLY tolerance: <<: *NORMAL_TOLERANCE - step: "ORE_LIFTER_DOWN" @@ -772,7 +775,7 @@ steps_list: arm: joints: [ *JOINT1_SMALL_READY_STORE, *JOINT2_SMALL_READY, *JOINT3_SMALL_TURN, *JOINT4_R_POSITION, *JOINT5_MID_POSITION, *JOINT6_UP_POSITION, *JOINT7_MID_POSITION ] common: - <<: *QUICKLY + <<: *NORMALLY tolerance: <<: *NORMAL_TOLERANCE - step: "CHASSIS_BACK" @@ -1103,7 +1106,7 @@ steps_list: arm: joints: [ *JOINT1_DOWN_POSITION, *JOINT2_EXCHANGE_POSITION, *JOINT3_MID_POSITION, *JOINT4_MID_POSITION, *JOINT5_MID_POSITION, *JOINT6_UP_POSITION, *JOINT7_MID_POSITION ] common: - <<: *QUICKLY + <<: *NORMALLY tolerance: <<: *NORMAL_TOLERANCE - step: "ORE_LIFTER_DOWN" @@ -1123,9 +1126,9 @@ steps_list: - step: "ORE_ROTATOR_READY" ore_rotator: target: *EXCHANGE_POS - - step: "roe lifter ready" - ore_lifter: - target: *BIN_SMALL_UP_POS +# - step: "roe lifter ready" +# ore_lifter: +# target: *BIN_SMALL_UP_POS - step: "JOINT2 move out" arm: joints: [ "KEEP", *JOINT2_AVOID_COLLISION, "KEEP", "KEEP", "KEEP", "KEEP", "KEEP" ] @@ -1168,7 +1171,7 @@ steps_list: arm: joints: [ *JOINT1_DOWN_POSITION, *JOINT2_EXCHANGE_POSITION, *JOINT3_MID_POSITION, *JOINT4_MID_POSITION, *JOINT5_MID_POSITION, *JOINT6_UP_POSITION, *JOINT7_MID_POSITION ] common: - <<: *QUICKLY + <<: *NORMALLY tolerance: <<: *NORMAL_TOLERANCE - step: "ORE_LIFTER_DOWN" @@ -1260,7 +1263,7 @@ steps_list: arm: joints: [ "KEEP", *JOINT2_AVOID_COLLISION, "KEEP", "KEEP", "KEEP", "KEEP", "KEEP" ] common: - <<: *QUICKLY + <<: *NORMALLY tolerance: <<: *NORMAL_TOLERANCE - step: "READY_TO_GET" @@ -1646,7 +1649,7 @@ steps_list: arm: joints: [ *JOINT1_DOWN_POSITION, *JOINT2_BACK_POSITION, "KEEP", *JOINT4_R_POSITION, *JOINT5_MID_POSITION, *JOINT6_UP_POSITION, *JOINT7_MID_POSITION ] common: - <<: *QUICKLY + <<: *NORMALLY tolerance: <<: *NORMAL_TOLERANCE - step: "pre_home" From 51f0fb3c95738d126d74764d179967d380b73dc3 Mon Sep 17 00:00:00 2001 From: cc0h <7921166012@qq.com> Date: Sat, 11 May 2024 17:53:19 +0800 Subject: [PATCH 27/80] record steps list. --- engineer_middleware/config/steps_list.yaml | 39 +++++++++++++--------- 1 file changed, 23 insertions(+), 16 deletions(-) diff --git a/engineer_middleware/config/steps_list.yaml b/engineer_middleware/config/steps_list.yaml index b42d2a0..1562acf 100644 --- a/engineer_middleware/config/steps_list.yaml +++ b/engineer_middleware/config/steps_list.yaml @@ -38,9 +38,9 @@ common: up_pos: &JOINT1_UP_POS 0.312 bin_get_stone_left: &JOINT1_BIN_GET_STONE_LEFT - 0.232 + 0.2 bin_get_stone_right: &JOINT1_BIN_GET_STONE_RIGHT - 0.144 + 0.1131 big_ready: &JOINT1_BIG_READY 0.006 big_adjust_mid: &JOINT1_BIG_ADJUST_MID @@ -50,7 +50,7 @@ common: big_ready_store: &JOINT1_BIG_READY_STORE 0.133 bin_get_stone: &JOINT1_BIN_GET_STONE - 0.1668 + 0.143 exchange_pos: &JOINT1_EXCHANGE_POS 0.1 home: @@ -80,7 +80,7 @@ common: bin_get_stone: &JOINT2_BIN_GET_STONE 0.13 back_bin_position: &JOINT2_BACK_BIN_POSITION - 0.05 + 0.07 avoid_collision: &JOINT2_AVOID_COLLISION 0.1 big_ready: &JOINT2_BIG_READY @@ -154,7 +154,7 @@ common: right_90_position: &JOINT5_R90_POSITION 1.59 bin_get_stone: &JOINT5_BIN_GET_STONE - 1.679 + 1.603 big_ready: &JOINT5_BIG_READY -0.017 big_ready_store: &JOINT5_BIG_READY_STORE @@ -216,9 +216,9 @@ common: exchange: &JOINT7_EXCHANGE -1.771 bin_get_left: &JOINT7_BIN_GET_LEFT - -1.526 + -1.5774 bin_get_right: &JOINT7_BIN_GET_RIGHT - 1.594 + 1.68 gimbal: right_pos: &RIGHT_POS @@ -321,13 +321,13 @@ common: ready_pos: &READY_POS 0.001 exchange_pos: &EXCHANGE_POS - 1.59 + 1.6 ore_lifter: bin_up_pos: &BIN_UP_POS - 0.2 + 0.22 small_up_pos: &BIN_SMALL_UP_POS - 0.03 + 0.01 bin_mid_pos: &BIN_MID_POS 0.1 bin_down_pos: &BIN_DOWN_POS @@ -1078,6 +1078,7 @@ steps_list: <<: *QUICKLY tolerance: <<: *NORMAL_TOLERANCE + GET_DOWN_STONE_BIN0: - step: "OPEN_GRIPPER" gripper: <<: *OPEN_GRIPPER @@ -1085,7 +1086,7 @@ steps_list: arm: joints: [ "KEEP", *JOINT2_BACK_BIN_POSITION, "KEEP", "KEEP", "KEEP", "KEEP", "KEEP" ] common: - <<: *NORMALLY + <<: *SLOWLY tolerance: <<: *NORMAL_TOLERANCE - step: "JOINT1_UP" @@ -1126,9 +1127,9 @@ steps_list: - step: "ORE_ROTATOR_READY" ore_rotator: target: *EXCHANGE_POS -# - step: "roe lifter ready" -# ore_lifter: -# target: *BIN_SMALL_UP_POS + - step: "roe lifter ready" + ore_lifter: + target: *BIN_SMALL_UP_POS - step: "JOINT2 move out" arm: joints: [ "KEEP", *JOINT2_AVOID_COLLISION, "KEEP", "KEEP", "KEEP", "KEEP", "KEEP" ] @@ -1143,6 +1144,7 @@ steps_list: <<: *QUICKLY tolerance: <<: *NORMAL_TOLERANCE + GET_UP_STONE_BIN0: - step: "OPEN_GRIPPER" gripper: <<: *OPEN_GRIPPER @@ -1153,6 +1155,7 @@ steps_list: <<: *NORMALLY tolerance: <<: *NORMAL_TOLERANCE + GET_UP_STONE_BIN00: - step: "JOINT1_UP" arm: joints: [ *JOINT1_UP_POS, "KEEP", "KEEP", "KEEP", "KEEP", "KEEP", "KEEP" ] @@ -1208,6 +1211,7 @@ steps_list: <<: *QUICKLY tolerance: <<: *NORMAL_TOLERANCE + GET_DOWN_STONE_LEFT0: - step: "OPEN_GRIPPER" gripper: <<: *OPEN_GRIPPER @@ -1215,7 +1219,7 @@ steps_list: arm: joints: [ "KEEP", *JOINT2_BACK_BIN_POSITION, "KEEP", "KEEP", "KEEP", "KEEP", "KEEP" ] common: - <<: *NORMALLY + <<: *SLOWLY tolerance: <<: *NORMAL_TOLERANCE - step: "JOINT1_UP" @@ -1273,6 +1277,7 @@ steps_list: <<: *QUICKLY tolerance: <<: *NORMAL_TOLERANCE + GET_DOWN_STONE_RIGHT0: - step: "OPEN_GRIPPER" gripper: <<: *OPEN_GRIPPER @@ -1280,7 +1285,7 @@ steps_list: arm: joints: [ "KEEP", *JOINT2_BACK_BIN_POSITION, "KEEP", "KEEP", "KEEP", "KEEP", "KEEP" ] common: - <<: *NORMALLY + <<: *SLOWLY tolerance: <<: *NORMAL_TOLERANCE - step: "JOINT1_UP" @@ -1338,6 +1343,7 @@ steps_list: <<: *QUICKLY tolerance: <<: *NORMAL_TOLERANCE + GET_UP_STONE_LEFT0: - step: "OPEN_GRIPPER" gripper: <<: *OPEN_GRIPPER @@ -1403,6 +1409,7 @@ steps_list: <<: *QUICKLY tolerance: <<: *NORMAL_TOLERANCE + GET_UP_STONE_RIGHT0: - step: "OPEN_GRIPPER" gripper: <<: *OPEN_GRIPPER From ab06274d79ae316291ea1d30f310cee38cb8a180 Mon Sep 17 00:00:00 2001 From: cc0h <7921166012@qq.com> Date: Mon, 13 May 2024 02:40:57 +0800 Subject: [PATCH 28/80] adjust max vel. --- engineer_middleware/config/engineer.yaml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/engineer_middleware/config/engineer.yaml b/engineer_middleware/config/engineer.yaml index fb1caf4..24d6f86 100644 --- a/engineer_middleware/config/engineer.yaml +++ b/engineer_middleware/config/engineer.yaml @@ -6,4 +6,4 @@ chassis: yaw: pid: { p: 5., i: 0.0, d: 0.0, i_clamp_max: 0, i_clamp_min: 0, antiwindup: true, publish_state: true } yaw_start_threshold: 0.05 - max_vel: 0.1 + max_vel: 10 From 5c4f322408e9174738a58ced0eea179a460c640d Mon Sep 17 00:00:00 2001 From: cc0h <7921166012@qq.com> Date: Mon, 13 May 2024 02:41:36 +0800 Subject: [PATCH 29/80] adjust motion order. --- engineer_middleware/config/steps_list.yaml | 209 ++++++++------------- 1 file changed, 79 insertions(+), 130 deletions(-) diff --git a/engineer_middleware/config/steps_list.yaml b/engineer_middleware/config/steps_list.yaml index 1562acf..a667e61 100644 --- a/engineer_middleware/config/steps_list.yaml +++ b/engineer_middleware/config/steps_list.yaml @@ -15,11 +15,11 @@ common: tolerance: small_tolerance: &SMALL_TOLERANCE - tolerance_joints: [ 0.005, 0.008, 0.015, 0.1, 0.1, 0.1, 0.05 ] + tolerance_joints: [ 0.005, 0.008, 0.015, 0.1, 0.1, 0.1, 0.03 ] normal_tolerance: &NORMAL_TOLERANCE - tolerance_joints: [ 0.01, 0.010, 0.015, 0.3, 0.2, 0.15, 0.07 ] + tolerance_joints: [ 0.01, 0.010, 0.02, 0.3, 0.3, 0.15, 0.05 ] bigger_tolerance: &BIGGER_TOLERANCE - tolerance_joints: [ 0.015, 0.015, 0.02, 0.3, 0.2, 0.2, 0.2 ] + tolerance_joints: [ 0.015, 0.015, 0.02, 0.3, 0.2, 0.2, 0.1 ] max_tolerance: &MAX_TOLERANCE tolerance_joints: [ 0.03, 0.03, 0.03, 0.3, 0.3, 0.3 , 0.3] @@ -30,7 +30,7 @@ common: small_ready: &JOINT1_SMALL_READY 0.487 small_down: &JOINT1_SMALL_DOWN - 0.31 + 0.3677 small_ready_store: &JOINT1_SMALL_READY_STORE 0.254 small_ready_store_third: &JOINT1_SMALL_READY_STORE_THIRD @@ -50,7 +50,7 @@ common: big_ready_store: &JOINT1_BIG_READY_STORE 0.133 bin_get_stone: &JOINT1_BIN_GET_STONE - 0.143 + 0.1521 exchange_pos: &JOINT1_EXCHANGE_POS 0.1 home: @@ -76,11 +76,11 @@ common: small_ready: &JOINT2_SMALL_READY 0.029 small_store: &JOINT2_SMALL_STORE - 0.044 + 0.064040 bin_get_stone: &JOINT2_BIN_GET_STONE 0.13 back_bin_position: &JOINT2_BACK_BIN_POSITION - 0.07 + 0.06651 avoid_collision: &JOINT2_AVOID_COLLISION 0.1 big_ready: &JOINT2_BIG_READY @@ -109,7 +109,7 @@ common: small_turn: &JOINT3_SMALL_TURN 0.257 bin_get_stone: &JOINT3_BIN_GET_STONE - 0.252 + 0.2608 bin_get_lv4_stone: &JOINT3_BIN_GET_LV4_STONE 0.31 big_ready: &JOINT3_BIG_READY @@ -154,7 +154,7 @@ common: right_90_position: &JOINT5_R90_POSITION 1.59 bin_get_stone: &JOINT5_BIN_GET_STONE - 1.603 + 1.5798 big_ready: &JOINT5_BIG_READY -0.017 big_ready_store: &JOINT5_BIG_READY_STORE @@ -185,7 +185,7 @@ common: small_ready_store: &JOINT6_SMALL_READY_STORE -1.633 bin_get_stone: &JOINT6_BIN_GET_STONE - -1.611 + -1.5936 big_ready: &JOINT6_BIG_READY 0.014 big_adjust_mid: &JOINT6_BIG_ADJUST_MID @@ -200,7 +200,7 @@ common: joint7: mechanical: mid_position: &JOINT7_MID_POSITION - 0.044 + -0.098300 left_90_position: &JOINT7_L90_POSITION -1.56 right_90_position: &JOINT7_R90_POSITION @@ -215,10 +215,12 @@ common: 1.722 exchange: &JOINT7_EXCHANGE -1.771 + lv4_ore: &JOINT7_LV4_ORE + -0.098300 bin_get_left: &JOINT7_BIN_GET_LEFT - -1.5774 + -1.6973 bin_get_right: &JOINT7_BIN_GET_RIGHT - 1.68 + 1.554 gimbal: right_pos: &RIGHT_POS @@ -292,6 +294,7 @@ common: chassis_tolerance_angular: 0.3 common: timeout: 2. + chassis_180: &CHASSIS_180 frame: base_link position: [ 0., 0. ] @@ -300,6 +303,7 @@ common: chassis_tolerance_angular: 0.2 common: timeout: 2. + chassis_90: &CHASSIS_TURN frame: base_link position: [ 0., 0. ] @@ -308,6 +312,7 @@ common: chassis_tolerance_angular: 0.2 common: timeout: 2. + chassis_back: &CHASSIS_BACK frame: base_link position: [ -0.4, 0. ] @@ -679,7 +684,7 @@ steps_list: common: <<: *NORMALLY tolerance: - <<: *NORMAL_TOLERANCE + <<: *BIGGER_TOLERANCE - step: "CLOSE_GRIPPER" gripper: <<: *CLOSE_GRIPPER @@ -698,7 +703,6 @@ steps_list: change: "SILVER" #####SECOND_STONE######## -# TWO_STONE_SMALL_ISLAND00: - step: "GIMBAL_READY" gimbal: <<: *FRONT_POS @@ -718,7 +722,6 @@ steps_list: - step: "OPEN_GRIPPER" gripper: <<: *OPEN_GRIPPER -# TWO_STONE_SMALL_ISLAND000: - step: "GET_STONE" arm: joints: [ *JOINT1_SMALL_DOWN, *JOINT2_SMALL_READY, *JOINT3_SMALL_READY_MID, *JOINT4_SMALL_READY, *JOINT5_SMALL_READY, *JOINT6_SMALL_READY, *JOINT7_SMALL_READY ] @@ -767,32 +770,33 @@ steps_list: common: <<: *NORMALLY tolerance: - <<: *NORMAL_TOLERANCE + <<: *BIGGER_TOLERANCE - step: "CLOSE_GRIPPER" gripper: <<: *CLOSE_GRIPPER - - step: "MOVE_ARM_OUT" - arm: - joints: [ *JOINT1_SMALL_READY_STORE, *JOINT2_SMALL_READY, *JOINT3_SMALL_TURN, *JOINT4_R_POSITION, *JOINT5_MID_POSITION, *JOINT6_UP_POSITION, *JOINT7_MID_POSITION ] - common: - <<: *NORMALLY - tolerance: - <<: *NORMAL_TOLERANCE + - step: "ADD_STONE" + stone_num: + change: "SILVER" - step: "CHASSIS_BACK" chassis: <<: *CHASSIS_BACK - step: "CHASSIS_TURN" chassis: <<: *CHASSIS_TURN + - step: "MOVE_ARM_OUT" + arm: + joints: [ *JOINT1_SMALL_READY_STORE, *JOINT2_SMALL_READY, *JOINT3_SMALL_TURN, *JOINT4_R_POSITION, *JOINT5_MID_POSITION, *JOINT6_UP_POSITION, *JOINT7_MID_POSITION ] + common: + <<: *NORMALLY + tolerance: + <<: *BIGGER_TOLERANCE - step: "pre_home" arm: <<: *PRE_HOME - step: "home" arm: <<: *HOME - - step: "ADD_STONE" - stone_num: - change: "SILVER" + ######THREE_STONE_ONCE(yi jian san lian)######### ######FIRST_STONE######## @@ -1068,7 +1072,7 @@ steps_list: arm: joints: [ "KEEP", *JOINT2_AVOID_COLLISION, "KEEP", "KEEP", "KEEP", "KEEP", "KEEP" ] common: - <<: *NORMALLY + <<: *SLOWLY tolerance: <<: *NORMAL_TOLERANCE - step: "READY_TO_GET" @@ -1078,7 +1082,6 @@ steps_list: <<: *QUICKLY tolerance: <<: *NORMAL_TOLERANCE - GET_DOWN_STONE_BIN0: - step: "OPEN_GRIPPER" gripper: <<: *OPEN_GRIPPER @@ -1091,14 +1094,7 @@ steps_list: <<: *NORMAL_TOLERANCE - step: "JOINT1_UP" arm: - joints: [ *JOINT1_UP_POS, "KEEP", "KEEP", "KEEP", "KEEP", "KEEP", "KEEP" ] - common: - <<: *QUICKLY - tolerance: - <<: *NORMAL_TOLERANCE - - step: "JOINT5_LIFT_STONE" - arm: - joints: [ "KEEP", "KEEP", "KEEP", "KEEP", *JOINT5_MID_POSITION, "KEEP", "KEEP" ] + joints: [ *JOINT1_UP_POS, "KEEP", "KEEP", "KEEP", *JOINT5_MID_POSITION, "KEEP", "KEEP" ] common: <<: *QUICKLY tolerance: @@ -1110,15 +1106,16 @@ steps_list: <<: *NORMALLY tolerance: <<: *NORMAL_TOLERANCE + - step: "remove stone" + stone_num: + change: "-1" - step: "ORE_LIFTER_DOWN" ore_lifter: target: *BIN_DOWN_POS - step: "gimbal lifter tallest" gimbal_lifter: target: *TALLEST_POS - - step: "remove stone" - stone_num: - change: "-1" + GET_UP_STONE_BIN: - step: "GIMBAL_READY" @@ -1134,17 +1131,16 @@ steps_list: arm: joints: [ "KEEP", *JOINT2_AVOID_COLLISION, "KEEP", "KEEP", "KEEP", "KEEP", "KEEP" ] common: - <<: *NORMALLY + <<: *SLOWLY tolerance: <<: *NORMAL_TOLERANCE - step: "READY_TO_GET" arm: joints: [ *JOINT1_BIN_GET_STONE, *JOINT2_BIN_GET_STONE, *JOINT3_BIN_GET_LV4_STONE, *JOINT4_R_POSITION, *JOINT5_BIN_GET_STONE, *JOINT6_BIN_GET_STONE, *JOINT7_MID_POSITION ] common: - <<: *QUICKLY + <<: *NORMALLY tolerance: <<: *NORMAL_TOLERANCE - GET_UP_STONE_BIN0: - step: "OPEN_GRIPPER" gripper: <<: *OPEN_GRIPPER @@ -1155,17 +1151,9 @@ steps_list: <<: *NORMALLY tolerance: <<: *NORMAL_TOLERANCE - GET_UP_STONE_BIN00: - step: "JOINT1_UP" arm: - joints: [ *JOINT1_UP_POS, "KEEP", "KEEP", "KEEP", "KEEP", "KEEP", "KEEP" ] - common: - <<: *QUICKLY - tolerance: - <<: *NORMAL_TOLERANCE - - step: "JOINT5_LIFT_STONE" - arm: - joints: [ "KEEP", "KEEP", "KEEP", "KEEP", *JOINT5_MID_POSITION, "KEEP", "KEEP" ] + joints: [ *JOINT1_UP_POS, "KEEP", "KEEP", "KEEP", *JOINT5_MID_POSITION, "KEEP", "KEEP" ] common: <<: *QUICKLY tolerance: @@ -1177,15 +1165,16 @@ steps_list: <<: *NORMALLY tolerance: <<: *NORMAL_TOLERANCE + - step: "remove stone" + stone_num: + change: "-1" - step: "ORE_LIFTER_DOWN" ore_lifter: target: *BIN_DOWN_POS - step: "gimbal lifter tallest" gimbal_lifter: target: *TALLEST_POS - - step: "remove stone" - stone_num: - change: "-1" + GET_DOWN_STONE_LEFT: - step: "GIMBAL_READY" @@ -1201,17 +1190,16 @@ steps_list: arm: joints: [ "KEEP", *JOINT2_AVOID_COLLISION, "KEEP", "KEEP", "KEEP", "KEEP", "KEEP" ] common: - <<: *NORMALLY + <<: *SLOWLY tolerance: <<: *NORMAL_TOLERANCE - step: "READY_TO_GET" arm: joints: [ *JOINT1_BIN_GET_STONE_LEFT, *JOINT2_BIN_GET_STONE, *JOINT3_BIN_GET_STONE, *JOINT4_R_POSITION, *JOINT5_BIN_GET_STONE, *JOINT6_BIN_GET_STONE, *JOINT7_BIN_GET_LEFT ] common: - <<: *QUICKLY + <<: *NORMALLY tolerance: - <<: *NORMAL_TOLERANCE - GET_DOWN_STONE_LEFT0: + <<: *BIGGER_TOLERANCE - step: "OPEN_GRIPPER" gripper: <<: *OPEN_GRIPPER @@ -1224,18 +1212,14 @@ steps_list: <<: *NORMAL_TOLERANCE - step: "JOINT1_UP" arm: - joints: [ *JOINT1_UP_POS, "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" ] + joints: [ *JOINT1_UP_POS, "KEEP", "KEEP", "KEEP", *JOINT5_MID_POSITION, "KEEP", "KEEP" ] common: <<: *NORMALLY tolerance: <<: *NORMAL_TOLERANCE + - step: "remove stone" + stone_num: + change: "-1" - step: "exchange pos" arm: joints: [*JOINT1_DOWN_POSITION, *JOINT2_EXCHANGE_POSITION, *JOINT3_MID_POSITION, *JOINT4_MID_POSITION, *JOINT5_MID_POSITION, *JOINT6_UP_POSITION, *JOINT7_MID_POSITION] @@ -1249,9 +1233,7 @@ steps_list: - step: "gimbal lifter tallest" gimbal_lifter: target: *TALLEST_POS - - step: "remove stone" - stone_num: - change: "-1" + GET_DOWN_STONE_RIGHT: - step: "GIMBAL_READY" @@ -1267,17 +1249,16 @@ steps_list: arm: joints: [ "KEEP", *JOINT2_AVOID_COLLISION, "KEEP", "KEEP", "KEEP", "KEEP", "KEEP" ] common: - <<: *NORMALLY + <<: *SLOWLY tolerance: <<: *NORMAL_TOLERANCE - step: "READY_TO_GET" arm: joints: [ *JOINT1_BIN_GET_STONE_RIGHT, *JOINT2_BIN_GET_STONE, *JOINT3_BIN_GET_STONE, *JOINT4_R_POSITION, *JOINT5_BIN_GET_STONE, *JOINT6_BIN_GET_STONE, *JOINT7_BIN_GET_RIGHT ] common: - <<: *QUICKLY + <<: *NORMALLY tolerance: - <<: *NORMAL_TOLERANCE - GET_DOWN_STONE_RIGHT0: + <<: *BIGGER_TOLERANCE - step: "OPEN_GRIPPER" gripper: <<: *OPEN_GRIPPER @@ -1290,14 +1271,7 @@ steps_list: <<: *NORMAL_TOLERANCE - step: "JOINT1_UP" arm: - joints: [ *JOINT1_UP_POS, "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" ] + joints: [ *JOINT1_UP_POS, "KEEP", "KEEP", "KEEP", *JOINT5_MID_POSITION, "KEEP", "KEEP" ] common: <<: *NORMALLY tolerance: @@ -1309,15 +1283,16 @@ steps_list: <<: *NORMALLY tolerance: <<: *NORMAL_TOLERANCE + - step: "remove stone" + stone_num: + change: "-1" - step: "ORE_LIFTER_DOWN" ore_lifter: target: *BIN_DOWN_POS - step: "gimbal lifter tallest" gimbal_lifter: target: *TALLEST_POS - - step: "remove stone" - stone_num: - change: "-1" + GET_UP_STONE_LEFT: - step: "GIMBAL_READY" @@ -1333,17 +1308,16 @@ steps_list: arm: joints: [ "KEEP", *JOINT2_AVOID_COLLISION, "KEEP", "KEEP", "KEEP", "KEEP", "KEEP" ] common: - <<: *NORMALLY + <<: *SLOWLY tolerance: <<: *NORMAL_TOLERANCE - step: "READY_TO_GET" arm: joints: [ *JOINT1_BIN_GET_STONE_LEFT, *JOINT2_BIN_GET_STONE, *JOINT3_BIN_GET_STONE, *JOINT4_R_POSITION, *JOINT5_BIN_GET_STONE, *JOINT6_BIN_GET_STONE, *JOINT7_BIN_GET_LEFT ] common: - <<: *QUICKLY + <<: *NORMALLY tolerance: - <<: *NORMAL_TOLERANCE - GET_UP_STONE_LEFT0: + <<: *BIGGER_TOLERANCE - step: "OPEN_GRIPPER" gripper: <<: *OPEN_GRIPPER @@ -1351,19 +1325,12 @@ steps_list: arm: joints: [ "KEEP", *JOINT2_BACK_BIN_POSITION, "KEEP", "KEEP", "KEEP", "KEEP", "KEEP" ] common: - <<: *NORMALLY + <<: *SLOWLY tolerance: <<: *NORMAL_TOLERANCE - step: "JOINT1_UP" arm: - joints: [ *JOINT1_UP_POS, "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" ] + joints: [ *JOINT1_UP_POS, "KEEP", "KEEP", "KEEP", *JOINT5_MID_POSITION, "KEEP", "KEEP" ] common: <<: *NORMALLY tolerance: @@ -1375,15 +1342,16 @@ steps_list: <<: *NORMALLY tolerance: <<: *NORMAL_TOLERANCE + - step: "remove stone" + stone_num: + change: "-1" - step: "ORE_LIFTER_DOWN" ore_lifter: target: *BIN_DOWN_POS - step: "gimbal lifter tallest" gimbal_lifter: target: *TALLEST_POS - - step: "remove stone" - stone_num: - change: "-1" + GET_UP_STONE_RIGHT: - step: "GIMBAL_READY" @@ -1399,17 +1367,16 @@ steps_list: arm: joints: [ "KEEP", *JOINT2_AVOID_COLLISION, "KEEP", "KEEP", "KEEP", "KEEP", "KEEP" ] common: - <<: *NORMALLY + <<: *SLOWLY tolerance: <<: *NORMAL_TOLERANCE - step: "READY_TO_GET" arm: joints: [ *JOINT1_BIN_GET_STONE_RIGHT, *JOINT2_BIN_GET_STONE, *JOINT3_BIN_GET_STONE, *JOINT4_R_POSITION, *JOINT5_BIN_GET_STONE, *JOINT6_BIN_GET_STONE, *JOINT7_BIN_GET_RIGHT ] common: - <<: *QUICKLY + <<: *NORMALLY tolerance: - <<: *NORMAL_TOLERANCE - GET_UP_STONE_RIGHT0: + <<: *BIGGER_TOLERANCE - step: "OPEN_GRIPPER" gripper: <<: *OPEN_GRIPPER @@ -1417,21 +1384,14 @@ steps_list: arm: joints: [ "KEEP", *JOINT2_BACK_BIN_POSITION, "KEEP", "KEEP", "KEEP", "KEEP", "KEEP" ] common: - <<: *NORMALLY + <<: *SLOWLY tolerance: <<: *NORMAL_TOLERANCE - step: "JOINT1_UP" arm: - joints: [ *JOINT1_UP_POS, "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" ] + joints: [ *JOINT1_UP_POS, "KEEP", "KEEP", "KEEP", *JOINT5_MID_POSITION, "KEEP", "KEEP" ] common: - <<: *NORMALLY + <<: *QUICKLY tolerance: <<: *NORMAL_TOLERANCE - step: "exchange pos" @@ -1441,15 +1401,16 @@ steps_list: <<: *NORMALLY tolerance: <<: *NORMAL_TOLERANCE + - step: "remove stone" + stone_num: + change: "-1" - step: "ORE_LIFTER_DOWN" ore_lifter: target: *BIN_DOWN_POS - step: "gimbal lifter tallest" gimbal_lifter: target: *TALLEST_POS - - step: "remove stone" - stone_num: - change: "-1" + #################BIG_ISLAND############# ######MID_STONE####### @@ -1471,9 +1432,6 @@ steps_list: - step: "ORE_ROTATOR_READY" ore_rotator: target: *EXCHANGE_POS -# - step: "ORE_LIFTER_READY" -# ore_lifter: -# target: *BIN_MID_POS - step: "gimbal lifter" gimbal_lifter: target: *LIFTER_BIG_ISLAND @@ -1502,9 +1460,6 @@ 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" ] @@ -1586,9 +1541,6 @@ steps_list: - step: "ORE_ROTATOR_READY" ore_rotator: target: *EXCHANGE_POS -# - step: "ORE_LIFTER_READY" -# ore_lifter: -# target: *BIN_MID_POS - step: "gimbal lifter" gimbal_lifter: target: *LIFTER_BIG_ISLAND @@ -1618,9 +1570,6 @@ 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" ] From e548a091abe51c44a0939d03f175cf5542607b34 Mon Sep 17 00:00:00 2001 From: cc0h <7921166012@qq.com> Date: Mon, 13 May 2024 21:46:13 +0800 Subject: [PATCH 30/80] adjust tolerance. --- engineer_middleware/config/steps_list.yaml | 46 +++++++++++++++------- 1 file changed, 31 insertions(+), 15 deletions(-) diff --git a/engineer_middleware/config/steps_list.yaml b/engineer_middleware/config/steps_list.yaml index a667e61..74a8a9f 100644 --- a/engineer_middleware/config/steps_list.yaml +++ b/engineer_middleware/config/steps_list.yaml @@ -477,8 +477,7 @@ steps_list: common: <<: *NORMALLY tolerance: - <<: *NORMAL_TOLERANCE -# ONE_STONE_SMALL_ISLAND00: + <<: *BIGGER_TOLERANCE - step: "CLOSE_GRIPPER" gripper: <<: *CLOSE_GRIPPER @@ -488,7 +487,7 @@ steps_list: common: <<: *NORMALLY tolerance: - <<: *NORMAL_TOLERANCE + <<: *BIGGER_TOLERANCE - step: "CHASSIS_BACK" chassis: <<: *CHASSIS_BACK @@ -579,8 +578,7 @@ steps_list: common: <<: *NORMALLY tolerance: - <<: *NORMAL_TOLERANCE - # ONE_STONE_SMALL_ISLAND00: + <<: *BIGGER_TOLERANCE - step: "CLOSE_GRIPPER" gripper: <<: *CLOSE_GRIPPER @@ -777,12 +775,6 @@ steps_list: - step: "ADD_STONE" stone_num: change: "SILVER" - - step: "CHASSIS_BACK" - chassis: - <<: *CHASSIS_BACK - - step: "CHASSIS_TURN" - chassis: - <<: *CHASSIS_TURN - step: "MOVE_ARM_OUT" arm: joints: [ *JOINT1_SMALL_READY_STORE, *JOINT2_SMALL_READY, *JOINT3_SMALL_TURN, *JOINT4_R_POSITION, *JOINT5_MID_POSITION, *JOINT6_UP_POSITION, *JOINT7_MID_POSITION ] @@ -790,6 +782,12 @@ steps_list: <<: *NORMALLY tolerance: <<: *BIGGER_TOLERANCE + - step: "CHASSIS_BACK" + chassis: + <<: *CHASSIS_BACK + - step: "CHASSIS_TURN" + chassis: + <<: *CHASSIS_TURN - step: "pre_home" arm: <<: *PRE_HOME @@ -1424,7 +1422,7 @@ steps_list: 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 ] + joints: [ *JOINT1_BIG_READY, *JOINT2_BIG_READY, *JOINT3_BIG_READY, *JOINT4_R_POSITION, *JOINT5_BIG_READY, *JOINT6_BIG_READY ] common: <<: *QUICKLY tolerance: @@ -1460,6 +1458,9 @@ steps_list: <<: *SLOWLY tolerance: <<: *NORMAL_TOLERANCE + - step: "left" + chassis: + <<: *CHASSIS_LEFT_15 - step: "JOINT1_UP" arm: joints: [ *JOINT1_BIG_UP_POS, "KEEP", "KEEP", "KEEP", "KEEP", "KEEP", "KEEP" ] @@ -1492,6 +1493,9 @@ steps_list: - step: "CLOSE_GRIPPER" gripper: <<: *CLOSE_GRIPPER + - step: "add stone" + stone_num: + change: "GOLD" - 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 ] @@ -1499,6 +1503,12 @@ steps_list: <<: *QUICKLY tolerance: <<: *NORMAL_TOLERANCE + - step: "chassis left" + chassis: + <<: *CHASSIS_LEFT_15 + - step: "chassis turn" + chassis: + <<: *CHASSIS_180 - step: "pre_home" arm: <<: *PRE_HOME @@ -1515,9 +1525,6 @@ steps_list: extend_arm: front: *EX_CLOSE back: *EX_CLOSE - - step: "add stone" - stone_num: - change: "GOLD" - step: "gimbal front" gimbal: <<: *FRONT_POS @@ -1570,6 +1577,9 @@ steps_list: <<: *SLOWLY tolerance: <<: *NORMAL_TOLERANCE + - step: "left" + chassis: + <<: *CHASSIS_LEFT_15 - step: "JOINT1_UP" arm: joints: [ *JOINT1_BIG_UP_POS, "KEEP", "KEEP", "KEEP", "KEEP", "KEEP", "KEEP" ] @@ -1617,6 +1627,12 @@ steps_list: - step: "ORE_LIFTER_DOWN" ore_lifter: target: *BIN_DOWN_POS + - step: "chassis left" + chassis: + <<: *CHASSIS_LEFT_15 + - step: "chassis turn" + chassis: + <<: *CHASSIS_180 - step: "ORE_ROTATOR_RESET" ore_rotator: target: *READY_POS From 9d0a14f0569b2a9798bfed9ee492decbc641fed9 Mon Sep 17 00:00:00 2001 From: cc0h <7921166012@qq.com> Date: Wed, 15 May 2024 01:05:35 +0800 Subject: [PATCH 31/80] adjust tolerance. --- engineer_middleware/config/steps_list.yaml | 202 +++++++++++++++------ 1 file changed, 150 insertions(+), 52 deletions(-) diff --git a/engineer_middleware/config/steps_list.yaml b/engineer_middleware/config/steps_list.yaml index 74a8a9f..a290985 100644 --- a/engineer_middleware/config/steps_list.yaml +++ b/engineer_middleware/config/steps_list.yaml @@ -17,9 +17,9 @@ common: small_tolerance: &SMALL_TOLERANCE tolerance_joints: [ 0.005, 0.008, 0.015, 0.1, 0.1, 0.1, 0.03 ] normal_tolerance: &NORMAL_TOLERANCE - tolerance_joints: [ 0.01, 0.010, 0.02, 0.3, 0.3, 0.15, 0.05 ] + tolerance_joints: [ 0.01, 0.010, 0.02, 0.35, 0.3, 0.15, 0.05 ] bigger_tolerance: &BIGGER_TOLERANCE - tolerance_joints: [ 0.015, 0.015, 0.02, 0.3, 0.2, 0.2, 0.1 ] + tolerance_joints: [ 0.015, 0.015, 0.02, 0.35, 0.2, 0.2, 0.1 ] max_tolerance: &MAX_TOLERANCE tolerance_joints: [ 0.03, 0.03, 0.03, 0.3, 0.3, 0.3 , 0.3] @@ -33,6 +33,8 @@ common: 0.3677 small_ready_store: &JOINT1_SMALL_READY_STORE 0.254 + small_store_down: &JOINT1_SMALL_STORE_DOWN + 0.1962 small_ready_store_third: &JOINT1_SMALL_READY_STORE_THIRD 0.387 up_pos: &JOINT1_UP_POS @@ -76,7 +78,7 @@ common: small_ready: &JOINT2_SMALL_READY 0.029 small_store: &JOINT2_SMALL_STORE - 0.064040 + 0.060290 bin_get_stone: &JOINT2_BIN_GET_STONE 0.13 back_bin_position: &JOINT2_BACK_BIN_POSITION @@ -102,6 +104,8 @@ common: 0.08 small_ready_mid: &JOINT3_SMALL_READY_MID 0.032 + take_one: &JOINT3_TAKE_ONE + -0.052850 small_ready_left: &JOINT3_SMALL_READY_LEFT 0.308 small_ready_right: &JOINT3_SMALL_READY_RIGHT @@ -135,6 +139,8 @@ common: -1.554 small_ready: &JOINT4_SMALL_READY 0.054 + to: &JOINT4_TAKE_ONE + 0.828930 small_turn: &JOINT4_SMALL_TURN -1.497 big_store: &JOINT4_BIG_STORE @@ -166,7 +172,7 @@ common: small_up_stone: &JOINT5_SMALL_UP_STONE -0.001 small_store: &JOINT5_SMALL_STORE - 1.615 + 1.4662 exchange: &JOINT5_EXCHANGE 1.805 @@ -219,6 +225,8 @@ common: -0.098300 bin_get_left: &JOINT7_BIN_GET_LEFT -1.6973 + to: &JOINT7_TAKE_ONE + -0.844500 bin_get_right: &JOINT7_BIN_GET_RIGHT 1.554 @@ -363,6 +371,34 @@ steps_list: - step: "gimbal lifter tallest" gimbal_lifter: target: *TALLEST_POS + EXCHANGE_R: + - step: "gimbal look front" + gimbal: + <<: *FRONT_POS + - step: "move arm to exchange pos" + arm: + joints: [ *JOINT1_EXCHANGE_POS, *JOINT2_EXCHANGE_POSITION, *JOINT3_MID_POSITION, *JOINT4_MID_POSITION, *JOINT5_MID_POSITION, *JOINT6_UP_POSITION, *JOINT7_BIN_GET_LEFT ] + common: + <<: *NORMALLY + tolerance: + <<: *NORMAL_TOLERANCE + - step: "gimbal lifter tallest" + gimbal_lifter: + target: *TALLEST_POS + EXCHANGE_L: + - step: "gimbal look front" + gimbal: + <<: *FRONT_POS + - step: "move arm to exchange pos" + arm: + joints: [ *JOINT1_EXCHANGE_POS, *JOINT2_EXCHANGE_POSITION, *JOINT3_MID_POSITION, *JOINT4_MID_POSITION, *JOINT5_MID_POSITION, *JOINT6_UP_POSITION, *JOINT7_BIN_GET_RIGHT ] + common: + <<: *NORMALLY + tolerance: + <<: *NORMAL_TOLERANCE + - step: "gimbal lifter tallest" + gimbal_lifter: + target: *TALLEST_POS ############ MID ############# @@ -404,6 +440,74 @@ steps_list: <<: *CLOSE_GRIPPER ################### SMALL_ISLAND_FROM_UP ################ + + ##### TAKE ONE USE ONE ##### + + TAKE_ONE_USE_ONE: + - step: "GIMBAL_READY" + gimbal: + <<: *FRONT_POS + - step: "ORE_ROTATOR_READY" + ore_rotator: + target: *READY_POS + - step: "ORE_LIFTER_READY" + ore_lifter: + target: *BIN_DOWN_POS + - step: "ready to get ore" + arm: + joints: [ *JOINT1_SMALL_READY, *JOINT2_SMALL_READY, *JOINT3_TAKE_ONE, *JOINT4_TAKE_ONE, *JOINT5_SMALL_READY, *JOINT6_SMALL_READY, *JOINT7_TAKE_ONE ] + common: + <<: *QUICKLY + tolerance: + <<: *NORMAL_TOLERANCE + - step: "OPEN_GRIPPER" + gripper: + <<: *OPEN_GRIPPER + - step: "gimbal lifter ready" + gimbal_lifter: + target: *LIFTER_SMALL_ISLAND + TAKE_ONE_USE_ONE0: + - step: "GET_STONE" + arm: + joints: [ *JOINT1_SMALL_DOWN, *JOINT2_SMALL_READY, *JOINT3_TAKE_ONE, *JOINT4_TAKE_ONE, *JOINT5_SMALL_READY, *JOINT6_SMALL_READY, *JOINT7_TAKE_ONE ] + common: + <<: *NORMALLY + tolerance: + <<: *NORMAL_TOLERANCE + - step: "JOINT1_UP" + arm: + joints: [ *JOINT1_SMALL_READY, "KEEP", "KEEP", "KEEP", "KEEP", "KEEP", "KEEP" ] + common: + <<: *NORMALLY + tolerance: + <<: *BIGGER_TOLERANCE + - step: "CHASSIS_BACK" + chassis: + <<: *CHASSIS_BACK + - step: "CHASSIS_TURN" + chassis: + <<: *CHASSIS_TURN + - step: "gimbal lifter ready" + gimbal_lifter: + target: *TALLEST_POS + - step: "ROTATE_STONE" + arm: + joints: [ *JOINT1_SMALL_DOWN, *JOINT2_SMALL_READY, *JOINT3_TAKE_ONE, *JOINT4_TAKE_ONE, *JOINT5_SMALL_READY, *JOINT6_MID_POSITION, *JOINT7_TAKE_ONE ] + common: + <<: *NORMALLY + tolerance: + <<: *BIGGER_TOLERANCE + - step: "move arm to exchange pos" + arm: + joints: [ *JOINT1_EXCHANGE_POS, *JOINT2_EXCHANGE_POSITION, *JOINT3_MID_POSITION, *JOINT4_MID_POSITION, *JOINT5_MID_POSITION, *JOINT6_UP_POSITION, *JOINT7_MID_POSITION ] + common: + <<: *NORMALLY + tolerance: + <<: *NORMAL_TOLERANCE + + + + ########ONE_STONE_ONCE######### 0_TAKE_ONE_STONE_SMALL_ISLAND: - step: "GIMBAL_READY" @@ -414,7 +518,7 @@ steps_list: target: *READY_POS - step: "ORE_LIFTER_READY" ore_lifter: - target: *BIN_MID_POS + target: *BIN_DOWN_POS - step: "ARM_READY" arm: joints: [ *JOINT1_SMALL_READY, *JOINT2_SMALL_READY, *JOINT3_SMALL_READY_MID, *JOINT4_SMALL_READY, *JOINT5_SMALL_READY, *JOINT6_SMALL_READY, *JOINT7_SMALL_READY ] @@ -588,7 +692,7 @@ steps_list: common: <<: *NORMALLY tolerance: - <<: *NORMAL_TOLERANCE + <<: *BIGGER_TOLERANCE - step: "CHASSIS_BACK" chassis: <<: *CHASSIS_BACK @@ -619,7 +723,7 @@ steps_list: target: *READY_POS - step: "ORE_LIFTER_READY" ore_lifter: - target: *BIN_MID_POS + target: *BIN_DOWN_POS - step: "ARM_READY" arm: joints: [ *JOINT1_SMALL_READY, *JOINT2_SMALL_READY, *JOINT3_SMALL_READY_LEFT, *JOINT4_SMALL_READY, *JOINT5_SMALL_READY, *JOINT6_SMALL_READY, *JOINT7_SMALL_READY ] @@ -666,14 +770,14 @@ steps_list: arm: joints: [ *JOINT1_SMALL_READY, *JOINT2_SMALL_READY, *JOINT3_SMALL_TURN, *JOINT4_SMALL_TURN, *JOINT5_SMALL_UP_STONE, *JOINT6_SMALL_READY, *JOINT7_SMALL_READY ] common: - <<: *QUICKLY + <<: *NORMALLY tolerance: <<: *NORMAL_TOLERANCE - step: "READY_STORE" arm: joints: [ *JOINT1_SMALL_READY_STORE, *JOINT2_SMALL_STORE, *JOINT3_SMALL_TURN, *JOINT4_SMALL_TURN, *JOINT5_SMALL_UP_STONE, *JOINT6_SMALL_READY_STORE, *JOINT7_SMALL_READY_STORE ] common: - <<: *QUICKLY + <<: *NORMALLY tolerance: <<: *NORMAL_TOLERANCE - step: "STORE" @@ -683,6 +787,9 @@ steps_list: <<: *NORMALLY tolerance: <<: *BIGGER_TOLERANCE + - step: "ADD_STONE" + stone_num: + change: "SILVER" - step: "CLOSE_GRIPPER" gripper: <<: *CLOSE_GRIPPER @@ -696,9 +803,7 @@ steps_list: - step: "ORE_LIFTER_DOWN" ore_lifter: target: *BIN_DOWN_POS - - step: "ADD_STONE" - stone_num: - change: "SILVER" + #####SECOND_STONE######## - step: "GIMBAL_READY" @@ -759,7 +864,7 @@ steps_list: arm: joints: [ *JOINT1_SMALL_READY_STORE, *JOINT2_SMALL_STORE, *JOINT3_SMALL_TURN, *JOINT4_SMALL_TURN, *JOINT5_SMALL_UP_STONE, *JOINT6_SMALL_READY_STORE, *JOINT7_SMALL_READY_STORE ] common: - <<: *QUICKLY + <<: *NORMALLY tolerance: <<: *NORMAL_TOLERANCE - step: "STORE" @@ -769,12 +874,12 @@ steps_list: <<: *NORMALLY tolerance: <<: *BIGGER_TOLERANCE - - step: "CLOSE_GRIPPER" - gripper: - <<: *CLOSE_GRIPPER - step: "ADD_STONE" stone_num: change: "SILVER" + - step: "CLOSE_GRIPPER" + gripper: + <<: *CLOSE_GRIPPER - step: "MOVE_ARM_OUT" arm: joints: [ *JOINT1_SMALL_READY_STORE, *JOINT2_SMALL_READY, *JOINT3_SMALL_TURN, *JOINT4_R_POSITION, *JOINT5_MID_POSITION, *JOINT6_UP_POSITION, *JOINT7_MID_POSITION ] @@ -1422,7 +1527,7 @@ steps_list: 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 ] + joints: [ *JOINT1_BIG_READY, *JOINT2_BIG_READY, *JOINT3_BIG_READY, *JOINT4_R_POSITION, *JOINT5_BIG_READY, *JOINT6_BIG_READY , *JOINT7_BIG_POS] common: <<: *QUICKLY tolerance: @@ -1443,7 +1548,7 @@ steps_list: common: <<: *NORMALLY tolerance: - <<: *NORMAL_TOLERANCE + <<: *BIGGER_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 ] @@ -1458,9 +1563,10 @@ steps_list: <<: *SLOWLY tolerance: <<: *NORMAL_TOLERANCE - - step: "left" - chassis: - <<: *CHASSIS_LEFT_15 + - step: "gimbal front" + gimbal: + <<: *FRONT_POS + MID_BIG_ISLAND00: - step: "JOINT1_UP" arm: joints: [ *JOINT1_BIG_UP_POS, "KEEP", "KEEP", "KEEP", "KEEP", "KEEP", "KEEP" ] @@ -1489,7 +1595,6 @@ steps_list: <<: *NORMALLY tolerance: <<: *NORMAL_TOLERANCE -# MID_BIG_ISLAND00: - step: "CLOSE_GRIPPER" gripper: <<: *CLOSE_GRIPPER @@ -1500,21 +1605,9 @@ steps_list: arm: joints: [ *JOINT1_DOWN_POSITION, *JOINT2_BACK_POSITION, "KEEP", *JOINT4_R_POSITION, *JOINT5_MID_POSITION, *JOINT6_UP_POSITION, *JOINT7_MID_POSITION ] common: - <<: *QUICKLY + <<: *NORMALLY tolerance: <<: *NORMAL_TOLERANCE - - step: "chassis left" - chassis: - <<: *CHASSIS_LEFT_15 - - step: "chassis turn" - chassis: - <<: *CHASSIS_180 - - step: "pre_home" - arm: - <<: *PRE_HOME - - step: "home" - arm: - <<: *HOME - step: "ORE_LIFTER_DOWN" ore_lifter: target: *BIN_DOWN_POS @@ -1525,6 +1618,13 @@ steps_list: extend_arm: front: *EX_CLOSE back: *EX_CLOSE + - step: "pre_home" + arm: + <<: *PRE_HOME + - step: "home" + arm: + <<: *HOME + - step: "gimbal front" gimbal: <<: *FRONT_POS @@ -1562,7 +1662,7 @@ steps_list: common: <<: *NORMALLY tolerance: - <<: *NORMAL_TOLERANCE + <<: *BIGGER_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 ] @@ -1577,9 +1677,10 @@ steps_list: <<: *SLOWLY tolerance: <<: *NORMAL_TOLERANCE - - step: "left" - chassis: - <<: *CHASSIS_LEFT_15 + - step: "gimbal front" + gimbal: + <<: *FRONT_POS + SIDE_BIG_ISLAND00: - step: "JOINT1_UP" arm: joints: [ *JOINT1_BIG_UP_POS, "KEEP", "KEEP", "KEEP", "KEEP", "KEEP", "KEEP" ] @@ -1608,6 +1709,9 @@ steps_list: <<: *NORMALLY tolerance: <<: *NORMAL_TOLERANCE + - step: "add stone" + stone_num: + change: "GOLD" - step: "CLOSE_GRIPPER" gripper: <<: *CLOSE_GRIPPER @@ -1617,7 +1721,10 @@ steps_list: common: <<: *NORMALLY tolerance: - <<: *NORMAL_TOLERANCE + <<: *BIGGER_TOLERANCE + - step: "gimbal front" + gimbal: + <<: *FRONT_POS - step: "pre_home" arm: <<: *PRE_HOME @@ -1627,12 +1734,6 @@ steps_list: - step: "ORE_LIFTER_DOWN" ore_lifter: target: *BIN_DOWN_POS - - step: "chassis left" - chassis: - <<: *CHASSIS_LEFT_15 - - step: "chassis turn" - chassis: - <<: *CHASSIS_180 - step: "ORE_ROTATOR_RESET" ore_rotator: target: *READY_POS @@ -1640,12 +1741,9 @@ steps_list: extend_arm: front: *EX_CLOSE back: *EX_CLOSE - - step: "add stone" - stone_num: - change: "GOLD" - - step: "gimbal front" - gimbal: - <<: *FRONT_POS + + + ################## STORE ################# From ec83643c7ddd8df91aa4bd1e022858f9dcd1c83c Mon Sep 17 00:00:00 2001 From: cc0h <7921166012@qq.com> Date: Thu, 16 May 2024 18:04:20 +0800 Subject: [PATCH 32/80] output joints with too big error. --- engineer_middleware/include/engineer_middleware/motion.h | 7 +++++-- 1 file changed, 5 insertions(+), 2 deletions(-) diff --git a/engineer_middleware/include/engineer_middleware/motion.h b/engineer_middleware/include/engineer_middleware/motion.h index 12de930..3ad3ff1 100644 --- a/engineer_middleware/include/engineer_middleware/motion.h +++ b/engineer_middleware/include/engineer_middleware/motion.h @@ -455,11 +455,14 @@ class JointMotion : public MoveitMotionBase { std::vector current = interface_.getCurrentJointValues(); double error = 0.; - bool flag = 1; + bool flag = 1, joint_reached = 0; for (int i = 0; i < (int)final_target_.size(); ++i) { error = std::abs(final_target_[i] - current[i]); - flag &= (error < tolerance_joints_[i]); + joint_reached = (error < tolerance_joints_[i]); + if (!joint_reached) + ROS_INFO_STREAM("Joint" << i + 1 << " didn't reach configured tolerance range,error: " << error); + flag &= joint_reached; } return flag; } From 5002c34a023b66452150ada764a4f6483e3c4564 Mon Sep 17 00:00:00 2001 From: cc0h <7921166012@qq.com> Date: Thu, 16 May 2024 18:04:35 +0800 Subject: [PATCH 33/80] modify store ore position. --- engineer_middleware/config/steps_list.yaml | 62 ++++++++++++++-------- 1 file changed, 39 insertions(+), 23 deletions(-) diff --git a/engineer_middleware/config/steps_list.yaml b/engineer_middleware/config/steps_list.yaml index a290985..9eebf9e 100644 --- a/engineer_middleware/config/steps_list.yaml +++ b/engineer_middleware/config/steps_list.yaml @@ -17,7 +17,7 @@ common: small_tolerance: &SMALL_TOLERANCE tolerance_joints: [ 0.005, 0.008, 0.015, 0.1, 0.1, 0.1, 0.03 ] normal_tolerance: &NORMAL_TOLERANCE - tolerance_joints: [ 0.01, 0.010, 0.02, 0.35, 0.3, 0.15, 0.05 ] + tolerance_joints: [ 0.012, 0.02, 0.015, 0.1, 0.2, 0.15, 0.05 ] bigger_tolerance: &BIGGER_TOLERANCE tolerance_joints: [ 0.015, 0.015, 0.02, 0.35, 0.2, 0.2, 0.1 ] max_tolerance: &MAX_TOLERANCE @@ -30,7 +30,7 @@ common: small_ready: &JOINT1_SMALL_READY 0.487 small_down: &JOINT1_SMALL_DOWN - 0.3677 + 0.34 small_ready_store: &JOINT1_SMALL_READY_STORE 0.254 small_store_down: &JOINT1_SMALL_STORE_DOWN @@ -44,9 +44,9 @@ common: bin_get_stone_right: &JOINT1_BIN_GET_STONE_RIGHT 0.1131 big_ready: &JOINT1_BIG_READY - 0.006 + 0.005 big_adjust_mid: &JOINT1_BIG_ADJUST_MID - 0.08 + 0.079 big_up_pos: &JOINT1_BIG_UP_POS 0.320 big_ready_store: &JOINT1_BIG_READY_STORE @@ -90,7 +90,7 @@ common: big_arm_up: &JOINT2_BIG_ARM_UP 0.142 big_ready_store: &JOINT2_BIG_READY_STORE - 0.055 + 0.0739 joint3: mechanical: @@ -103,7 +103,7 @@ common: home: &JOINT3_HOME_POS 0.08 small_ready_mid: &JOINT3_SMALL_READY_MID - 0.032 + 0.0402 take_one: &JOINT3_TAKE_ONE -0.052850 small_ready_left: &JOINT3_SMALL_READY_LEFT @@ -125,7 +125,7 @@ common: big_pull_out: &JOINT3_BIG_PULL_OUT 0.188 big_arm_up: &JOINT3_BIG_ARM_UP - 0.285 + 0.308 exchange: &JOINT3_EXCHANGE 0.081 @@ -160,7 +160,7 @@ common: right_90_position: &JOINT5_R90_POSITION 1.59 bin_get_stone: &JOINT5_BIN_GET_STONE - 1.5798 + 1.63912 big_ready: &JOINT5_BIG_READY -0.017 big_ready_store: &JOINT5_BIG_READY_STORE @@ -168,7 +168,7 @@ common: big_store: &JOINT5_BIG_STORE 1.49 small_ready: &JOINT5_SMALL_READY - -3.20 + -3.1267 small_up_stone: &JOINT5_SMALL_UP_STONE -0.001 small_store: &JOINT5_SMALL_STORE @@ -724,6 +724,12 @@ steps_list: - step: "ORE_LIFTER_READY" ore_lifter: target: *BIN_DOWN_POS + - step: "OPEN_GRIPPER" + gripper: + <<: *OPEN_GRIPPER + - step: "gimbal lifter ready" + gimbal_lifter: + target: *LIFTER_SMALL_ISLAND - step: "ARM_READY" arm: joints: [ *JOINT1_SMALL_READY, *JOINT2_SMALL_READY, *JOINT3_SMALL_READY_LEFT, *JOINT4_SMALL_READY, *JOINT5_SMALL_READY, *JOINT6_SMALL_READY, *JOINT7_SMALL_READY ] @@ -731,12 +737,7 @@ steps_list: <<: *QUICKLY tolerance: <<: *NORMAL_TOLERANCE - - step: "OPEN_GRIPPER" - gripper: - <<: *OPEN_GRIPPER - - step: "gimbal lifter ready" - gimbal_lifter: - target: *LIFTER_SMALL_ISLAND + TWO_STONE_SMALL_ISLAND0: - step: "GET_STONE" arm: @@ -1251,16 +1252,23 @@ steps_list: arm: joints: [ "KEEP", *JOINT2_BACK_BIN_POSITION, "KEEP", "KEEP", "KEEP", "KEEP", "KEEP" ] common: - <<: *NORMALLY + <<: *SLOWLY tolerance: <<: *NORMAL_TOLERANCE - step: "JOINT1_UP" arm: - joints: [ *JOINT1_UP_POS, "KEEP", "KEEP", "KEEP", *JOINT5_MID_POSITION, "KEEP", "KEEP" ] + joints: [ *JOINT1_UP_POS, "KEEP", "KEEP", "KEEP", "KEEP", "KEEP", "KEEP" ] common: <<: *QUICKLY tolerance: <<: *NORMAL_TOLERANCE + - step: "JOINT5_UP" + arm: + joints: [ "KEEP", "KEEP", "KEEP", "KEEP", *JOINT5_MID_POSITION, "KEEP", "KEEP" ] + common: + <<: *NORMALLY + tolerance: + <<: *NORMAL_TOLERANCE - step: "exchange pos" arm: joints: [ *JOINT1_DOWN_POSITION, *JOINT2_EXCHANGE_POSITION, *JOINT3_MID_POSITION, *JOINT4_MID_POSITION, *JOINT5_MID_POSITION, *JOINT6_UP_POSITION, *JOINT7_MID_POSITION ] @@ -1315,7 +1323,14 @@ steps_list: <<: *NORMAL_TOLERANCE - step: "JOINT1_UP" arm: - joints: [ *JOINT1_UP_POS, "KEEP", "KEEP", "KEEP", *JOINT5_MID_POSITION, "KEEP", "KEEP" ] + joints: [ *JOINT1_UP_POS, "KEEP", "KEEP", "KEEP", "KEEP", "KEEP", "KEEP" ] + common: + <<: *QUICKLY + tolerance: + <<: *NORMAL_TOLERANCE + - step: "JOINT5_UP" + arm: + joints: [ "KEEP", "KEEP", "KEEP", "KEEP", *JOINT5_MID_POSITION, "KEEP", "KEEP" ] common: <<: *NORMALLY tolerance: @@ -1549,13 +1564,14 @@ steps_list: <<: *NORMALLY tolerance: <<: *BIGGER_TOLERANCE + MID_BIG_ISLAND00: - 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 + <<: *BIGGER_TOLERANCE - step: "PULL_OUT_STONE" arm: joints: [ "KEEP", "KEEP", *JOINT3_BIG_PULL_OUT, "KEEP", "KEEP", "KEEP", "KEEP" ] @@ -1566,7 +1582,7 @@ steps_list: - step: "gimbal front" gimbal: <<: *FRONT_POS - MID_BIG_ISLAND00: + MID_BIG_ISLAND000: - step: "JOINT1_UP" arm: joints: [ *JOINT1_BIG_UP_POS, "KEEP", "KEEP", "KEEP", "KEEP", "KEEP", "KEEP" ] @@ -1702,6 +1718,9 @@ steps_list: <<: *NORMALLY tolerance: <<: *NORMAL_TOLERANCE + - step: "add stone" + stone_num: + change: "GOLD" - step: "STORE_STONE" arm: joints: [ *JOINT1_BIG_READY_STORE, *JOINT2_BIG_READY_STORE, *JOINT3_BIG_ARM_UP, *JOINT4_BIG_STORE, *JOINT5_BIG_STORE, *JOINT6_BIG_READY_STORE, *JOINT7_BIG_POS ] @@ -1709,9 +1728,6 @@ steps_list: <<: *NORMALLY tolerance: <<: *NORMAL_TOLERANCE - - step: "add stone" - stone_num: - change: "GOLD" - step: "CLOSE_GRIPPER" gripper: <<: *CLOSE_GRIPPER From c2530b0d6df57b27b94507c6704d3898ecaebd0f Mon Sep 17 00:00:00 2001 From: cc0h <7921166012@qq.com> Date: Fri, 17 May 2024 22:56:02 +0800 Subject: [PATCH 34/80] roll back joint7 position for small island ore. --- engineer_middleware/config/steps_list.yaml | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/engineer_middleware/config/steps_list.yaml b/engineer_middleware/config/steps_list.yaml index 9eebf9e..6f97576 100644 --- a/engineer_middleware/config/steps_list.yaml +++ b/engineer_middleware/config/steps_list.yaml @@ -115,7 +115,7 @@ common: bin_get_stone: &JOINT3_BIN_GET_STONE 0.2608 bin_get_lv4_stone: &JOINT3_BIN_GET_LV4_STONE - 0.31 + 0.294414 big_ready: &JOINT3_BIG_READY 0.071 get_big_mid: &JOINT3_GET_BIG_MID @@ -1640,7 +1640,6 @@ steps_list: - step: "home" arm: <<: *HOME - - step: "gimbal front" gimbal: <<: *FRONT_POS From 09aa3ecceda40219cb921f00976cd190170ca4c6 Mon Sep 17 00:00:00 2001 From: cc0h <7921166012@qq.com> Date: Sat, 18 May 2024 07:42:04 +0800 Subject: [PATCH 35/80] simplify small island motion. --- engineer_middleware/config/steps_list.yaml | 74 +++------------------- 1 file changed, 9 insertions(+), 65 deletions(-) diff --git a/engineer_middleware/config/steps_list.yaml b/engineer_middleware/config/steps_list.yaml index 6f97576..cd2f7a9 100644 --- a/engineer_middleware/config/steps_list.yaml +++ b/engineer_middleware/config/steps_list.yaml @@ -17,7 +17,7 @@ common: small_tolerance: &SMALL_TOLERANCE tolerance_joints: [ 0.005, 0.008, 0.015, 0.1, 0.1, 0.1, 0.03 ] normal_tolerance: &NORMAL_TOLERANCE - tolerance_joints: [ 0.012, 0.02, 0.015, 0.1, 0.2, 0.15, 0.05 ] + tolerance_joints: [ 0.012, 0.02, 0.015, 0.1, 0.2, 0.15, 0.07 ] bigger_tolerance: &BIGGER_TOLERANCE tolerance_joints: [ 0.015, 0.015, 0.02, 0.35, 0.2, 0.2, 0.1 ] max_tolerance: &MAX_TOLERANCE @@ -554,20 +554,6 @@ steps_list: <<: *NORMALLY tolerance: <<: *NORMAL_TOLERANCE - - step: "JOINT6_BACK" - arm: - joints: [ "KEEP", "KEEP", "KEEP", "KEEP", "KEEP", *JOINT6_SMALL_BACK, "KEEP" ] - common: - <<: *QUICKLY - tolerance: - <<: *NORMAL_TOLERANCE - - step: "TURN_STONE" - arm: - joints: [ *JOINT1_SMALL_READY, *JOINT2_SMALL_READY, *JOINT3_SMALL_TURN, *JOINT4_SMALL_TURN, *JOINT5_SMALL_UP_STONE, *JOINT6_SMALL_READY, *JOINT7_SMALL_READY ] - common: - <<: *QUICKLY - tolerance: - <<: *NORMAL_TOLERANCE - step: "READY_STORE" arm: joints: [ *JOINT1_SMALL_READY_STORE, *JOINT2_SMALL_STORE, *JOINT3_SMALL_TURN, *JOINT4_SMALL_TURN, *JOINT5_SMALL_UP_STONE, *JOINT6_SMALL_READY_STORE, *JOINT7_SMALL_READY_STORE ] @@ -655,20 +641,6 @@ steps_list: <<: *NORMALLY tolerance: <<: *NORMAL_TOLERANCE - - step: "JOINT6_BACK" - arm: - joints: [ "KEEP", "KEEP", "KEEP", "KEEP", "KEEP", *JOINT6_SMALL_BACK, "KEEP" ] - common: - <<: *QUICKLY - tolerance: - <<: *NORMAL_TOLERANCE - - step: "TURN_STONE" - arm: - joints: [ *JOINT1_SMALL_READY, *JOINT2_SMALL_READY, *JOINT3_SMALL_TURN, *JOINT4_SMALL_TURN, *JOINT5_SMALL_UP_STONE, *JOINT6_SMALL_READY, *JOINT7_SMALL_READY ] - common: - <<: *QUICKLY - tolerance: - <<: *NORMAL_TOLERANCE - step: "READY_STORE" arm: joints: [ *JOINT1_SMALL_READY_STORE, *JOINT2_SMALL_STORE, *JOINT3_SMALL_TURN, *JOINT4_SMALL_TURN, *JOINT5_SMALL_UP_STONE, *JOINT6_SMALL_READY_STORE, *JOINT7_SMALL_READY_STORE ] @@ -743,14 +715,14 @@ steps_list: arm: joints: [ *JOINT1_SMALL_DOWN, *JOINT2_SMALL_READY, *JOINT3_SMALL_READY_LEFT, *JOINT4_SMALL_READY, *JOINT5_SMALL_READY, *JOINT6_SMALL_READY, *JOINT7_SMALL_READY ] common: - <<: *NORMALLY + <<: *QUICKLY tolerance: <<: *NORMAL_TOLERANCE - step: "JOINT1_UP" arm: joints: [ *JOINT1_SMALL_READY, "KEEP", "KEEP", "KEEP", "KEEP", "KEEP", "KEEP" ] common: - <<: *NORMALLY + <<: *QUICKLY tolerance: <<: *NORMAL_TOLERANCE - step: "ARM_UP_STONE" @@ -760,25 +732,11 @@ steps_list: <<: *QUICKLY tolerance: <<: *NORMAL_TOLERANCE - - step: "JOINT6_BACK" - arm: - joints: [ "KEEP", "KEEP", "KEEP", "KEEP", "KEEP", *JOINT6_SMALL_BACK, "KEEP" ] - common: - <<: *QUICKLY - tolerance: - <<: *NORMAL_TOLERANCE - - step: "TURN_STONE" - arm: - joints: [ *JOINT1_SMALL_READY, *JOINT2_SMALL_READY, *JOINT3_SMALL_TURN, *JOINT4_SMALL_TURN, *JOINT5_SMALL_UP_STONE, *JOINT6_SMALL_READY, *JOINT7_SMALL_READY ] - common: - <<: *NORMALLY - tolerance: - <<: *NORMAL_TOLERANCE - step: "READY_STORE" arm: joints: [ *JOINT1_SMALL_READY_STORE, *JOINT2_SMALL_STORE, *JOINT3_SMALL_TURN, *JOINT4_SMALL_TURN, *JOINT5_SMALL_UP_STONE, *JOINT6_SMALL_READY_STORE, *JOINT7_SMALL_READY_STORE ] common: - <<: *NORMALLY + <<: *QUICKLY tolerance: <<: *NORMAL_TOLERANCE - step: "STORE" @@ -798,7 +756,7 @@ steps_list: arm: joints: [ *JOINT1_SMALL_READY_STORE, *JOINT2_SMALL_READY, *JOINT3_SMALL_TURN, *JOINT4_R_POSITION, *JOINT5_MID_POSITION, *JOINT6_UP_POSITION, *JOINT7_MID_POSITION ] common: - <<: *NORMALLY + <<: *QUICKLY tolerance: <<: *NORMAL_TOLERANCE - step: "ORE_LIFTER_DOWN" @@ -830,14 +788,14 @@ steps_list: arm: joints: [ *JOINT1_SMALL_DOWN, *JOINT2_SMALL_READY, *JOINT3_SMALL_READY_MID, *JOINT4_SMALL_READY, *JOINT5_SMALL_READY, *JOINT6_SMALL_READY, *JOINT7_SMALL_READY ] common: - <<: *NORMALLY + <<: *QUICKLY tolerance: <<: *NORMAL_TOLERANCE - step: "JOINT1_UP" arm: joints: [ *JOINT1_SMALL_READY, "KEEP", "KEEP", "KEEP", "KEEP", "KEEP", "KEEP" ] common: - <<: *NORMALLY + <<: *QUICKLY tolerance: <<: *NORMAL_TOLERANCE - step: "ARM_UP_STONE" @@ -847,25 +805,11 @@ steps_list: <<: *QUICKLY tolerance: <<: *NORMAL_TOLERANCE - - step: "JOINT6_BACK" - arm: - joints: [ "KEEP", "KEEP", "KEEP", "KEEP", "KEEP", *JOINT6_SMALL_BACK, "KEEP" ] - common: - <<: *QUICKLY - tolerance: - <<: *NORMAL_TOLERANCE - - step: "TURN_STONE" - arm: - joints: [ *JOINT1_SMALL_READY, *JOINT2_SMALL_READY, *JOINT3_SMALL_TURN, *JOINT4_SMALL_TURN, *JOINT5_SMALL_UP_STONE, *JOINT6_SMALL_READY, *JOINT7_SMALL_READY ] - common: - <<: *QUICKLY - tolerance: - <<: *NORMAL_TOLERANCE - step: "READY_STORE" arm: joints: [ *JOINT1_SMALL_READY_STORE, *JOINT2_SMALL_STORE, *JOINT3_SMALL_TURN, *JOINT4_SMALL_TURN, *JOINT5_SMALL_UP_STONE, *JOINT6_SMALL_READY_STORE, *JOINT7_SMALL_READY_STORE ] common: - <<: *NORMALLY + <<: *QUICKLY tolerance: <<: *NORMAL_TOLERANCE - step: "STORE" @@ -885,7 +829,7 @@ steps_list: arm: joints: [ *JOINT1_SMALL_READY_STORE, *JOINT2_SMALL_READY, *JOINT3_SMALL_TURN, *JOINT4_R_POSITION, *JOINT5_MID_POSITION, *JOINT6_UP_POSITION, *JOINT7_MID_POSITION ] common: - <<: *NORMALLY + <<: *QUICKLY tolerance: <<: *BIGGER_TOLERANCE - step: "CHASSIS_BACK" From 0d32e53f69f6eb7460a85d09fc32b3e3034f80f4 Mon Sep 17 00:00:00 2001 From: cc0h <7921166012@qq.com> Date: Mon, 3 Jun 2024 22:11:03 +0800 Subject: [PATCH 36/80] add engineer2_arm_connfig, modify launch files to select different config files. --- engineer2_arm_config/.setup_assistant | 11 + engineer2_arm_config/CMakeLists.txt | 10 + .../config/chomp_planning.yaml | 18 + engineer2_arm_config/config/engineer2.srdf | 55 ++ .../config/fake_controllers.yaml | 13 + engineer2_arm_config/config/joint_limits.yaml | 40 ++ engineer2_arm_config/config/kinematics.yaml | 3 + .../config/ompl_planning.yaml | 158 +++++ .../config/ros_controllers.yaml | 31 + engineer2_arm_config/config/sensors_3d.yaml | 2 + engineer2_arm_config/config/servo.yaml | 68 +++ .../launch/chomp_planning_pipeline.launch.xml | 23 + ...ineer_moveit_controller_manager.launch.xml | 10 + .../engineer_moveit_sensor_manager.launch.xml | 3 + .../fake_moveit_controller_manager.launch.xml | 9 + .../launch/load_controllers.launch | 49 ++ .../launch/load_move_group.launch | 18 + engineer2_arm_config/launch/move_group.launch | 65 +++ engineer2_arm_config/launch/moveit.rviz | 540 ++++++++++++++++++ .../ompl-chomp_planning_pipeline.launch.xml | 20 + .../launch/ompl_planning_pipeline.launch.xml | 25 + ...otion_planner_planning_pipeline.launch.xml | 15 + .../launch/planning_context.launch | 25 + .../launch/planning_pipeline.launch.xml | 10 + engineer2_arm_config/launch/rviz.launch | 14 + engineer2_arm_config/launch/servo.launch | 6 + .../launch/trajectory_execution.launch.xml | 21 + engineer2_arm_config/package.xml | 27 + engineer_middleware/config/engineer2.yaml | 9 + .../config/engineer2_steps_list.yaml | 0 ...eps_list.yaml => engineer_steps_list.yaml} | 0 engineer_middleware/launch/load.launch | 5 +- engineer_middleware/launch/sim.launch | 7 +- 33 files changed, 1305 insertions(+), 5 deletions(-) create mode 100644 engineer2_arm_config/.setup_assistant create mode 100644 engineer2_arm_config/CMakeLists.txt create mode 100644 engineer2_arm_config/config/chomp_planning.yaml create mode 100644 engineer2_arm_config/config/engineer2.srdf create mode 100644 engineer2_arm_config/config/fake_controllers.yaml create mode 100644 engineer2_arm_config/config/joint_limits.yaml create mode 100644 engineer2_arm_config/config/kinematics.yaml create mode 100644 engineer2_arm_config/config/ompl_planning.yaml create mode 100644 engineer2_arm_config/config/ros_controllers.yaml create mode 100644 engineer2_arm_config/config/sensors_3d.yaml create mode 100644 engineer2_arm_config/config/servo.yaml create mode 100644 engineer2_arm_config/launch/chomp_planning_pipeline.launch.xml create mode 100644 engineer2_arm_config/launch/engineer_moveit_controller_manager.launch.xml create mode 100644 engineer2_arm_config/launch/engineer_moveit_sensor_manager.launch.xml create mode 100644 engineer2_arm_config/launch/fake_moveit_controller_manager.launch.xml create mode 100644 engineer2_arm_config/launch/load_controllers.launch create mode 100644 engineer2_arm_config/launch/load_move_group.launch create mode 100644 engineer2_arm_config/launch/move_group.launch create mode 100644 engineer2_arm_config/launch/moveit.rviz create mode 100644 engineer2_arm_config/launch/ompl-chomp_planning_pipeline.launch.xml create mode 100644 engineer2_arm_config/launch/ompl_planning_pipeline.launch.xml create mode 100644 engineer2_arm_config/launch/pilz_industrial_motion_planner_planning_pipeline.launch.xml create mode 100644 engineer2_arm_config/launch/planning_context.launch create mode 100644 engineer2_arm_config/launch/planning_pipeline.launch.xml create mode 100644 engineer2_arm_config/launch/rviz.launch create mode 100644 engineer2_arm_config/launch/servo.launch create mode 100644 engineer2_arm_config/launch/trajectory_execution.launch.xml create mode 100644 engineer2_arm_config/package.xml create mode 100644 engineer_middleware/config/engineer2.yaml create mode 100644 engineer_middleware/config/engineer2_steps_list.yaml rename engineer_middleware/config/{steps_list.yaml => engineer_steps_list.yaml} (100%) diff --git a/engineer2_arm_config/.setup_assistant b/engineer2_arm_config/.setup_assistant new file mode 100644 index 0000000..197025b --- /dev/null +++ b/engineer2_arm_config/.setup_assistant @@ -0,0 +1,11 @@ +moveit_setup_assistant_config: + URDF: + package: rm_description + relative_path: urdf/engineer2/engineer2.urdf.xacro + xacro_args: "" + SRDF: + relative_path: config/engineer2.srdf + CONFIG: + author_name: QiayuanLiao + author_email: liaoqiayuan@gmail.com + generated_timestamp: 1617294211 diff --git a/engineer2_arm_config/CMakeLists.txt b/engineer2_arm_config/CMakeLists.txt new file mode 100644 index 0000000..ad88885 --- /dev/null +++ b/engineer2_arm_config/CMakeLists.txt @@ -0,0 +1,10 @@ +cmake_minimum_required(VERSION 3.1.3) +project(engineer2_arm_config) + +find_package(catkin REQUIRED) + +catkin_package() + +install(DIRECTORY launch DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} + PATTERN "setup_assistant.launch" EXCLUDE) +install(DIRECTORY config DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}) diff --git a/engineer2_arm_config/config/chomp_planning.yaml b/engineer2_arm_config/config/chomp_planning.yaml new file mode 100644 index 0000000..0831fda --- /dev/null +++ b/engineer2_arm_config/config/chomp_planning.yaml @@ -0,0 +1,18 @@ +planning_time_limit: 10.0 +max_iterations: 200 +max_iterations_after_collision_free: 5 +smoothness_cost_weight: 0.1 +obstacle_cost_weight: 1.0 +learning_rate: 0.01 +smoothness_cost_velocity: 0.0 +smoothness_cost_acceleration: 1.0 +smoothness_cost_jerk: 0.0 +ridge_factor: 0.01 +use_pseudo_inverse: false +pseudo_inverse_ridge_factor: 1e-4 +joint_update_limit: 0.1 +collision_clearance: 0.2 +collision_threshold: 0.07 +use_stochastic_descent: true +enable_failure_recovery: true +max_recovery_attempts: 15 diff --git a/engineer2_arm_config/config/engineer2.srdf b/engineer2_arm_config/config/engineer2.srdf new file mode 100644 index 0000000..a9012bb --- /dev/null +++ b/engineer2_arm_config/config/engineer2.srdf @@ -0,0 +1,55 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/engineer2_arm_config/config/fake_controllers.yaml b/engineer2_arm_config/config/fake_controllers.yaml new file mode 100644 index 0000000..c51e46f --- /dev/null +++ b/engineer2_arm_config/config/fake_controllers.yaml @@ -0,0 +1,13 @@ +controller_list: + - name: fake_engineer_arm_controller + type: $(arg fake_execution_type) + joints: + - joint1 + - joint2 + - joint3 + - joint4 + - joint5 + - joint6 +initial: # Define initial robot poses per group +# - group: engineer_arm +# pose: home diff --git a/engineer2_arm_config/config/joint_limits.yaml b/engineer2_arm_config/config/joint_limits.yaml new file mode 100644 index 0000000..f0cf0d3 --- /dev/null +++ b/engineer2_arm_config/config/joint_limits.yaml @@ -0,0 +1,40 @@ +# joint_limits.yaml allows the dynamics properties specified in the URDF to be overwritten or augmented as needed + +# For beginners, we downscale velocity and acceleration limits. +# You can always specify higher scaling factors (<= 1.0) in your motion requests. # Increase the values below to 1.0 to always move at maximum speed. +default_velocity_scaling_factor: 0.1 +default_acceleration_scaling_factor: 0.1 + +# Specific joint properties can be changed with the keys [max_position, min_position, max_velocity, max_acceleration] +# Joint limits can be turned off with [has_velocity_limits, has_acceleration_limits] +joint_limits: + joint1: + has_velocity_limits: true + max_velocity: 1.80 + has_acceleration_limits: true + max_acceleration: 3.5 + joint2: + has_velocity_limits: true + max_velocity: 2.75 + has_acceleration_limits: true + max_acceleration: 6 + joint3: + has_velocity_limits: true + max_velocity: 1.80 + has_acceleration_limits: true + max_acceleration: 5 + joint4: + has_velocity_limits: true + max_velocity: 19 + has_acceleration_limits: true + max_acceleration: 50 + joint5: + has_velocity_limits: true + max_velocity: 15 + has_acceleration_limits: true + max_acceleration: 20 + joint6: + has_velocity_limits: true + max_velocity: 50 + has_acceleration_limits: true + max_acceleration: 90 diff --git a/engineer2_arm_config/config/kinematics.yaml b/engineer2_arm_config/config/kinematics.yaml new file mode 100644 index 0000000..b298b37 --- /dev/null +++ b/engineer2_arm_config/config/kinematics.yaml @@ -0,0 +1,3 @@ +engineer_arm: + kinematics_solver: trac_ik_kinematics_plugin/TRAC_IKKinematicsPlugin + kinematics_solver_timeout: 0.005 diff --git a/engineer2_arm_config/config/ompl_planning.yaml b/engineer2_arm_config/config/ompl_planning.yaml new file mode 100644 index 0000000..8e69e09 --- /dev/null +++ b/engineer2_arm_config/config/ompl_planning.yaml @@ -0,0 +1,158 @@ +planner_configs: + AnytimePathShortening: + type: geometric::AnytimePathShortening + shortcut: true # Attempt to shortcut all new solution paths + hybridize: true # Compute hybrid solution trajectories + max_hybrid_paths: 40 # Number of hybrid paths generated per iteration + num_planners: 20 # The number of default planners to use for planning + planners: "" # A comma-separated list of planner types (e.g., "PRM,EST,RRTConnect"Optionally, planner parameters can be passed to change the default:"PRM[max_nearest_neighbors=5],EST[goal_bias=.5],RRT[range=10. goal_bias=.1]" + SBL: + type: geometric::SBL + range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() + EST: + type: geometric::EST + range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0 setup() + goal_bias: 0.05 # When close to goal select goal, with this probability. default: 0.05 + LBKPIECE: + type: geometric::LBKPIECE + range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() + border_fraction: 0.9 # Fraction of time focused on boarder default: 0.9 + min_valid_path_fraction: 0.5 # Accept partially valid moves above fraction. default: 0.5 + BKPIECE: + type: geometric::BKPIECE + range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() + border_fraction: 0.9 # Fraction of time focused on boarder default: 0.9 + failed_expansion_score_factor: 0.5 # When extending motion fails, scale score by factor. default: 0.5 + min_valid_path_fraction: 0.5 # Accept partially valid moves above fraction. default: 0.5 + KPIECE: + type: geometric::KPIECE + range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() + goal_bias: 0.05 # When close to goal select goal, with this probability. default: 0.05 + border_fraction: 0.9 # Fraction of time focused on boarder default: 0.9 (0.0,1.] + failed_expansion_score_factor: 0.5 # When extending motion fails, scale score by factor. default: 0.5 + min_valid_path_fraction: 0.5 # Accept partially valid moves above fraction. default: 0.5 + RRT: + type: geometric::RRT + range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() + goal_bias: 0.05 # When close to goal select goal, with this probability? default: 0.05 + RRTConnect: + type: geometric::RRTConnect + range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() + RRTstar: + type: geometric::RRTstar + range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() + goal_bias: 0.05 # When close to goal select goal, with this probability? default: 0.05 + delay_collision_checking: 1 # Stop collision checking as soon as C-free parent found. default 1 + TRRT: + type: geometric::TRRT + range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() + goal_bias: 0.05 # When close to goal select goal, with this probability? default: 0.05 + max_states_failed: 10 # when to start increasing temp. default: 10 + temp_change_factor: 2.0 # how much to increase or decrease temp. default: 2.0 + min_temperature: 10e-10 # lower limit of temp change. default: 10e-10 + init_temperature: 10e-6 # initial temperature. default: 10e-6 + frountier_threshold: 0.0 # dist new state to nearest neighbor to disqualify as frontier. default: 0.0 set in setup() + frountierNodeRatio: 0.1 # 1/10, or 1 nonfrontier for every 10 frontier. default: 0.1 + k_constant: 0.0 # value used to normalize expresssion. default: 0.0 set in setup() + PRM: + type: geometric::PRM + max_nearest_neighbors: 10 # use k nearest neighbors. default: 10 + PRMstar: + type: geometric::PRMstar + FMT: + type: geometric::FMT + num_samples: 1000 # number of states that the planner should sample. default: 1000 + radius_multiplier: 1.1 # multiplier used for the nearest neighbors search radius. default: 1.1 + nearest_k: 1 # use Knearest strategy. default: 1 + cache_cc: 1 # use collision checking cache. default: 1 + heuristics: 0 # activate cost to go heuristics. default: 0 + extended_fmt: 1 # activate the extended FMT*: adding new samples if planner does not finish successfully. default: 1 + BFMT: + type: geometric::BFMT + num_samples: 1000 # number of states that the planner should sample. default: 1000 + radius_multiplier: 1.0 # multiplier used for the nearest neighbors search radius. default: 1.0 + nearest_k: 1 # use the Knearest strategy. default: 1 + balanced: 0 # exploration strategy: balanced true expands one tree every iteration. False will select the tree with lowest maximum cost to go. default: 1 + optimality: 1 # termination strategy: optimality true finishes when the best possible path is found. Otherwise, the algorithm will finish when the first feasible path is found. default: 1 + heuristics: 1 # activates cost to go heuristics. default: 1 + cache_cc: 1 # use the collision checking cache. default: 1 + extended_fmt: 1 # Activates the extended FMT*: adding new samples if planner does not finish successfully. default: 1 + PDST: + type: geometric::PDST + STRIDE: + type: geometric::STRIDE + range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() + goal_bias: 0.05 # When close to goal select goal, with this probability. default: 0.05 + use_projected_distance: 0 # whether nearest neighbors are computed based on distances in a projection of the state rather distances in the state space itself. default: 0 + degree: 16 # desired degree of a node in the Geometric Near-neightbor Access Tree (GNAT). default: 16 + max_degree: 18 # max degree of a node in the GNAT. default: 12 + min_degree: 12 # min degree of a node in the GNAT. default: 12 + max_pts_per_leaf: 6 # max points per leaf in the GNAT. default: 6 + estimated_dimension: 0.0 # estimated dimension of the free space. default: 0.0 + min_valid_path_fraction: 0.2 # Accept partially valid moves above fraction. default: 0.2 + BiTRRT: + type: geometric::BiTRRT + range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() + temp_change_factor: 0.1 # how much to increase or decrease temp. default: 0.1 + init_temperature: 100 # initial temperature. default: 100 + frountier_threshold: 0.0 # dist new state to nearest neighbor to disqualify as frontier. default: 0.0 set in setup() + frountier_node_ratio: 0.1 # 1/10, or 1 nonfrontier for every 10 frontier. default: 0.1 + cost_threshold: 1e300 # the cost threshold. Any motion cost that is not better will not be expanded. default: inf + LBTRRT: + type: geometric::LBTRRT + range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() + goal_bias: 0.05 # When close to goal select goal, with this probability. default: 0.05 + epsilon: 0.4 # optimality approximation factor. default: 0.4 + BiEST: + type: geometric::BiEST + range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() + ProjEST: + type: geometric::ProjEST + range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() + goal_bias: 0.05 # When close to goal select goal, with this probability. default: 0.05 + LazyPRM: + type: geometric::LazyPRM + range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() + LazyPRMstar: + type: geometric::LazyPRMstar + SPARS: + type: geometric::SPARS + stretch_factor: 3.0 # roadmap spanner stretch factor. multiplicative upper bound on path quality. It does not make sense to make this parameter more than 3. default: 3.0 + sparse_delta_fraction: 0.25 # delta fraction for connection distance. This value represents the visibility range of sparse samples. default: 0.25 + dense_delta_fraction: 0.001 # delta fraction for interface detection. default: 0.001 + max_failures: 1000 # maximum consecutive failure limit. default: 1000 + SPARStwo: + type: geometric::SPARStwo + stretch_factor: 3.0 # roadmap spanner stretch factor. multiplicative upper bound on path quality. It does not make sense to make this parameter more than 3. default: 3.0 + sparse_delta_fraction: 0.25 # delta fraction for connection distance. This value represents the visibility range of sparse samples. default: 0.25 + dense_delta_fraction: 0.001 # delta fraction for interface detection. default: 0.001 + max_failures: 5000 # maximum consecutive failure limit. default: 5000 +engineer_arm: + default_planner_config: RRTConnect + planner_configs: + - AnytimePathShortening + - SBL + - EST + - LBKPIECE + - BKPIECE + - KPIECE + - RRT + - RRTConnect + - RRTstar + - TRRT + - PRM + - PRMstar + - FMT + - BFMT + - PDST + - STRIDE + - BiTRRT + - LBTRRT + - BiEST + - ProjEST + - LazyPRM + - LazyPRMstar + - SPARS + - SPARStwo + projection_evaluator: joints(joint1,joint2) + longest_valid_segment_fraction: 0.005 diff --git a/engineer2_arm_config/config/ros_controllers.yaml b/engineer2_arm_config/config/ros_controllers.yaml new file mode 100644 index 0000000..a4837dc --- /dev/null +++ b/engineer2_arm_config/config/ros_controllers.yaml @@ -0,0 +1,31 @@ +# Simulation settings for using moveit_sim_controllers +moveit_sim_hw_interface: + joint_model_group: engineer_arm + joint_model_group_pose: home +# Settings for ros_control_boilerplate control loop +generic_hw_control_loop: + loop_hz: 300 + cycle_time_error_threshold: 0.01 +# Settings for ros_control hardware interface +hardware_interface: + joints: + - joint1 + - joint2 + - joint3 + - joint4 + - joint5 + - joint6 + sim_control_mode: 1 # 0: position, 1: velocity + +controller_list: + - name: controllers/arm_trajectory_controller + action_ns: follow_joint_trajectory + default: True + type: FollowJointTrajectory + joints: + - joint1 + - joint2 + - joint3 + - joint4 + - joint5 + - joint6 diff --git a/engineer2_arm_config/config/sensors_3d.yaml b/engineer2_arm_config/config/sensors_3d.yaml new file mode 100644 index 0000000..add9f47 --- /dev/null +++ b/engineer2_arm_config/config/sensors_3d.yaml @@ -0,0 +1,2 @@ +sensors: + - { } diff --git a/engineer2_arm_config/config/servo.yaml b/engineer2_arm_config/config/servo.yaml new file mode 100644 index 0000000..76270c2 --- /dev/null +++ b/engineer2_arm_config/config/servo.yaml @@ -0,0 +1,68 @@ +############################################### +# Modify all parameters related to servoing here +############################################### +use_gazebo: true # Whether the robot is started in a Gazebo simulation environment + +## Properties of incoming commands +command_in_type: "speed_units" # "unitless"> in the range [-1:1], as if from joystick. "speed_units"> cmds are in m/s and rad/s +scale: + # Scale parameters are only used if command_in_type=="unitless" + linear: 0.4 # Max linear velocity. Meters per publish_period. Unit is [m/s]. Only used for Cartesian commands. + rotational: 0.2 # Max angular velocity. Rads per publish_period. Unit is [rad/s]. Only used for Cartesian commands. + # Max joint angular/linear velocity. Rads or Meters per publish period. Only used for joint commands on joint_command_in_topic. + joint: 0.005 +low_pass_filter_coeff: 1. # Larger --> trust the filtered data more, trust the measurements less. + +## Properties of outgoing commands +publish_period: 0.5 # 1/Nominal publish rate [seconds] +low_latency_mode: true # Set this to true to publish as soon as an incoming Twist command is received (publish_period is ignored) + +# What type of topic does your robot driver expect? +# Currently supported are std_msgs/Float64MultiArray (for ros_control JointGroupVelocityController or JointGroupPositionController) +# or trajectory_msgs/JointTrajectory (for Universal Robots and other non-ros_control robots) +command_out_type: trajectory_msgs/JointTrajectory + +# What to publish? Can save some bandwidth as most robots only require positions or velocities +publish_joint_positions: true +publish_joint_velocities: true +publish_joint_accelerations: false + +## MoveIt properties +move_group_name: engineer_arm # Often 'manipulator' or 'arm' +planning_frame: base_link # The MoveIt planning frame. Often 'base_link' or 'world' + +## Other frames +ee_frame_name: link6 # The name of the end effector link, used to return the EE pose +robot_link_command_frame: link6 # commands must be given in the frame of a robot link. Usually either the base or end effector + +## Stopping behaviour +incoming_command_timeout: 0.1 # Stop servoing if X seconds elapse without a new command +# If 0, republish commands forever even if the robot is stationary. Otherwise, specify num. to publish. +# Important because ROS may drop some messages and we need the robot to halt reliably. +num_outgoing_halt_msgs_to_publish: 4 + +## Configure handling of singularities and joint limits +lower_singularity_threshold: 17 # Start decelerating when the condition number hits this (close to singularity) +hard_stop_singularity_threshold: 30 # Stop when the condition number hits this +joint_limit_margin: 0.001 # added as a buffer to joint limits [radians]. If moving quickly, make this larger. + +## Topic names +cartesian_command_in_topic: delta_twist_cmds # Topic for incoming Cartesian twist commands +joint_command_in_topic: delta_joint_cmds # Topic for incoming joint angle commands +joint_topic: joint_states +status_topic: status # Publish status to this topic +command_out_topic: /controllers/arm_trajectory_controller/command # Publish outgoing commands here + +## Collision checking for the entire robot body +check_collisions: false # Check collisions? +collision_check_rate: 50 # [Hz] Collision-checking can easily bog down a CPU if done too often. +# Two collision check algorithms are available: +# "threshold_distance" begins slowing down when nearer than a specified distance. Good if you want to tune collision thresholds manually. +# "stop_distance" stops if a collision is nearer than the worst-case stopping distance and the distance is decreasing. Requires joint acceleration limits +collision_check_type: threshold_distance +# Parameters for "threshold_distance"-type collision checking +self_collision_proximity_threshold: 0.0002 # Start decelerating when a self-collision is this far [m] +scene_collision_proximity_threshold: 0.0005 # Start decelerating when a scene collision is this far [m] +# Parameters for "stop_distance"-type collision checking +collision_distance_safety_factor: 1000 # Must be >= 1. A large safety factor is recommended to account for latency +min_allowable_collision_distance: 0.001 # Stop if a collision is closer than this [m] diff --git a/engineer2_arm_config/launch/chomp_planning_pipeline.launch.xml b/engineer2_arm_config/launch/chomp_planning_pipeline.launch.xml new file mode 100644 index 0000000..2f60968 --- /dev/null +++ b/engineer2_arm_config/launch/chomp_planning_pipeline.launch.xml @@ -0,0 +1,23 @@ + + + + + + + + + + + + + + + + diff --git a/engineer2_arm_config/launch/engineer_moveit_controller_manager.launch.xml b/engineer2_arm_config/launch/engineer_moveit_controller_manager.launch.xml new file mode 100644 index 0000000..db614c0 --- /dev/null +++ b/engineer2_arm_config/launch/engineer_moveit_controller_manager.launch.xml @@ -0,0 +1,10 @@ + + + + + + + + + diff --git a/engineer2_arm_config/launch/engineer_moveit_sensor_manager.launch.xml b/engineer2_arm_config/launch/engineer_moveit_sensor_manager.launch.xml new file mode 100644 index 0000000..5d02698 --- /dev/null +++ b/engineer2_arm_config/launch/engineer_moveit_sensor_manager.launch.xml @@ -0,0 +1,3 @@ + + + diff --git a/engineer2_arm_config/launch/fake_moveit_controller_manager.launch.xml b/engineer2_arm_config/launch/fake_moveit_controller_manager.launch.xml new file mode 100644 index 0000000..d6865a4 --- /dev/null +++ b/engineer2_arm_config/launch/fake_moveit_controller_manager.launch.xml @@ -0,0 +1,9 @@ + + + + + + + + + diff --git a/engineer2_arm_config/launch/load_controllers.launch b/engineer2_arm_config/launch/load_controllers.launch new file mode 100644 index 0000000..69275b2 --- /dev/null +++ b/engineer2_arm_config/launch/load_controllers.launch @@ -0,0 +1,49 @@ + + + + + + + + + + + + + + + + + + + + + + diff --git a/engineer2_arm_config/launch/load_move_group.launch b/engineer2_arm_config/launch/load_move_group.launch new file mode 100644 index 0000000..a66a2a9 --- /dev/null +++ b/engineer2_arm_config/launch/load_move_group.launch @@ -0,0 +1,18 @@ + + + + + + + + + + + + + + + + + + diff --git a/engineer2_arm_config/launch/move_group.launch b/engineer2_arm_config/launch/move_group.launch new file mode 100644 index 0000000..3bc6450 --- /dev/null +++ b/engineer2_arm_config/launch/move_group.launch @@ -0,0 +1,65 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/engineer2_arm_config/launch/moveit.rviz b/engineer2_arm_config/launch/moveit.rviz new file mode 100644 index 0000000..a0a4838 --- /dev/null +++ b/engineer2_arm_config/launch/moveit.rviz @@ -0,0 +1,540 @@ +Panels: + - Class: rviz/Displays + Help Height: 84 + Name: Displays + Property Tree Widget: + Expanded: + - /MotionPlanning1 + - /MotionPlanning1/Planning Request1 + - /MotionPlanning1/Planning Metrics1 + - /MotionPlanning1/Planned Path1 + - /TF1 + - /TF1/Frames1 + Splitter Ratio: 0.5 + Tree Height: 315 + - Class: rviz/Help + Name: Help + - Class: rviz/Views + Expanded: + - /Current View1 + Name: Views + Splitter Ratio: 0.5 +Preferences: + PromptSaveOnExit: true +Toolbars: + toolButtonStyle: 2 +Visualization Manager: + Class: "" + Displays: + - Alpha: 0.5 + Cell Size: 1 + Class: rviz/Grid + Color: 160; 160; 164 + Enabled: true + Line Style: + Line Width: 0.029999999329447746 + Value: Lines + Name: Grid + Normal Cell Count: 0 + Offset: + X: 0 + Y: 0 + Z: 0 + Plane: XY + Plane Cell Count: 10 + Reference Frame: + Value: true + - Acceleration_Scaling_Factor: 0.1 + Class: moveit_rviz_plugin/MotionPlanning + Enabled: true + Move Group Namespace: "" + MoveIt_Allow_Approximate_IK: false + MoveIt_Allow_External_Program: false + MoveIt_Allow_Replanning: false + MoveIt_Allow_Sensor_Positioning: false + MoveIt_Planning_Attempts: 10 + MoveIt_Planning_Time: 5 + MoveIt_Use_Cartesian_Path: false + MoveIt_Use_Constraint_Aware_IK: false + MoveIt_Workspace: + Center: + X: 0 + Y: 0 + Z: 0 + Size: + X: 2 + Y: 2 + Z: 2 + Name: MotionPlanning + Planned Path: + Color Enabled: false + Interrupt Display: false + Links: + All Links Enabled: true + Expand Joint Details: false + Expand Link Details: false + Expand Tree: false + Link Tree Style: Links in Alphabetic Order + base_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + camera_link: + Alpha: 1 + Show Axes: false + Show Trail: false + camera_optical_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + extend_arm_back: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + extend_arm_front: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + gimbal_lifter: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + left_back_wheel: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + left_front_wheel: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + link1: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + link2: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + link3: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + link4: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + link5: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + link6: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + link7: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + mimic_link1: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + ore_bin_lifter: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + ore_bin_rotator: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + pitch: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + right_back_wheel: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + right_front_wheel: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + tools_link: + Alpha: 1 + Show Axes: false + Show Trail: false + tools_link_st: + Alpha: 1 + Show Axes: false + Show Trail: false + yaw: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + Loop Animation: false + Robot Alpha: 0.5 + Robot Color: 150; 50; 150 + Show Robot Collision: false + Show Robot Visual: true + Show Trail: false + State Display Time: 0.05 s + Trail Step Size: 1 + Trajectory Topic: move_group/display_planned_path + Use Sim Time: false + Planning Metrics: + Payload: 1 + Show Joint Torques: false + Show Manipulability: false + Show Manipulability Index: false + Show Weight Limit: false + TextHeight: 0.07999999821186066 + Planning Request: + Colliding Link Color: 255; 0; 0 + Goal State Alpha: 1 + Goal State Color: 250; 128; 0 + Interactive Marker Size: 0 + Joint Violation Color: 255; 0; 255 + Planning Group: engineer_arm + Query Goal State: true + Query Start State: false + Show Workspace: false + Start State Alpha: 1 + Start State Color: 0; 255; 0 + Planning Scene Topic: move_group/monitored_planning_scene + Robot Description: robot_description + Scene Geometry: + Scene Alpha: 1 + Scene Color: 50; 230; 50 + Scene Display Time: 0.009999999776482582 + Show Scene Geometry: true + Voxel Coloring: Z-Axis + Voxel Rendering: Occupied Voxels + Scene Robot: + Attached Body Color: 150; 50; 150 + Links: + All Links Enabled: true + Expand Joint Details: false + Expand Link Details: false + Expand Tree: false + Link Tree Style: Links in Alphabetic Order + base_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + camera_link: + Alpha: 1 + Show Axes: false + Show Trail: false + camera_optical_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + extend_arm_back: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + extend_arm_front: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + gimbal_lifter: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + left_back_wheel: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + left_front_wheel: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + link1: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + link2: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + link3: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + link4: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + link5: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + link6: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + link7: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + mimic_link1: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + ore_bin_lifter: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + ore_bin_rotator: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + pitch: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + right_back_wheel: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + right_front_wheel: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + tools_link: + Alpha: 1 + Show Axes: false + Show Trail: false + tools_link_st: + Alpha: 1 + Show Axes: false + Show Trail: false + yaw: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + Robot Alpha: 0.5 + Show Robot Collision: false + Show Robot Visual: true + Value: true + Velocity_Scaling_Factor: 0.1 + - Class: rviz/TF + Enabled: true + Filter (blacklist): "" + Filter (whitelist): "" + Frame Timeout: 15 + Frames: + All Enabled: false + base_link: + Value: true + camera_link: + Value: false + camera_optical_frame: + Value: false + exchanger: + Value: true + extend_arm_back: + Value: false + extend_arm_front: + Value: false + gimbal_lifter: + Value: false + left_back_wheel: + Value: false + left_front_wheel: + Value: false + link1: + Value: false + link2: + Value: false + link3: + Value: false + link4: + Value: false + link5: + Value: false + link6: + Value: false + link7: + Value: true + map: + Value: false + mimic_link1: + Value: false + odom: + Value: false + ore_bin_lifter: + Value: false + ore_bin_rotator: + Value: false + pitch: + Value: false + real_world: + Value: false + right_back_wheel: + Value: false + right_front_wheel: + Value: false + tools_link: + Value: false + tools_link_st: + Value: false + yaw: + Value: false + Marker Alpha: 1 + Marker Scale: 1 + Name: TF + Show Arrows: true + Show Axes: true + Show Names: true + Tree: + base_link: + {} + exchanger: + {} + extend_arm_back: + {} + extend_arm_front: + {} + gimbal_lifter: + camera_link: + {} + camera_optical_frame: + {} + left_back_wheel: + {} + left_front_wheel: + {} + link1: + {} + link2: + {} + link3: + {} + link4: + {} + link5: + {} + link6: + {} + link7: + tools_link: + {} + tools_link_st: + {} + map: + {} + mimic_link1: + {} + odom: + {} + ore_bin_lifter: + {} + ore_bin_rotator: + {} + pitch: + {} + real_world: + {} + right_back_wheel: + {} + right_front_wheel: + {} + yaw: + {} + Update Interval: 0 + Value: true + Enabled: true + Global Options: + Background Color: 48; 48; 48 + Default Light: true + Fixed Frame: base_link + Frame Rate: 30 + Name: root + Tools: + - Class: rviz/Interact + Hide Inactive Objects: true + - Class: rviz/MoveCamera + - Class: rviz/Select + Value: true + Views: + Current: + Class: rviz/Orbit + Distance: 1.4108070135116577 + Enable Stereo Rendering: + Stereo Eye Separation: 0.05999999865889549 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false + Field of View: 0.75 + Focal Point: + X: 0.23874804377555847 + Y: 0.04174364358186722 + Z: 0.6224369406700134 + Focal Shape Fixed Size: true + Focal Shape Size: 0.05000000074505806 + Invert Z Axis: false + Name: Current View + Near Clip Distance: 0.009999999776482582 + Pitch: 0.48979732394218445 + Target Frame: base_link + Yaw: 3.2827558517456055 + Saved: ~ +Window Geometry: + Displays: + collapsed: false + Height: 1016 + Help: + collapsed: false + Hide Left Dock: false + Hide Right Dock: false + MotionPlanning: + collapsed: false + MotionPlanning - Trajectory Slider: + collapsed: false + QMainWindow State: 000000ff00000000fd0000000100000000000001f30000039efc0200000007fb000000100044006900730070006c006100790073010000003d000001cc000000c900fffffffb0000000800480065006c00700000000342000000bb0000006e00fffffffb0000000a00560069006500770073000000010c000000a4000000a400fffffffb0000000c00430061006d00650072006100000002ff000001610000000000000000fb0000001e004d006f00740069006f006e00200050006c0061006e006e0069006e00670100000374000001890000000000000000fb00000044004d006f00740069006f006e0050006c0061006e006e0069006e00670020002d0020005400720061006a006500630074006f0072007900200053006c00690064006500720000000000ffffffff0000001600000016fb0000001c004d006f00740069006f006e0050006c0061006e006e0069006e0067010000020f000001cc0000017d00ffffff000004880000039e00000001000000020000000100000002fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + Views: + collapsed: false + Width: 1665 + X: 757 + Y: 27 diff --git a/engineer2_arm_config/launch/ompl-chomp_planning_pipeline.launch.xml b/engineer2_arm_config/launch/ompl-chomp_planning_pipeline.launch.xml new file mode 100644 index 0000000..a4531f3 --- /dev/null +++ b/engineer2_arm_config/launch/ompl-chomp_planning_pipeline.launch.xml @@ -0,0 +1,20 @@ + + + + + + + + + + + + diff --git a/engineer2_arm_config/launch/ompl_planning_pipeline.launch.xml b/engineer2_arm_config/launch/ompl_planning_pipeline.launch.xml new file mode 100644 index 0000000..c219994 --- /dev/null +++ b/engineer2_arm_config/launch/ompl_planning_pipeline.launch.xml @@ -0,0 +1,25 @@ + + + + + + + + + + + + + + + + + diff --git a/engineer2_arm_config/launch/pilz_industrial_motion_planner_planning_pipeline.launch.xml b/engineer2_arm_config/launch/pilz_industrial_motion_planner_planning_pipeline.launch.xml new file mode 100644 index 0000000..c7c4cf5 --- /dev/null +++ b/engineer2_arm_config/launch/pilz_industrial_motion_planner_planning_pipeline.launch.xml @@ -0,0 +1,15 @@ + + + + + + + + + + + + + + diff --git a/engineer2_arm_config/launch/planning_context.launch b/engineer2_arm_config/launch/planning_context.launch new file mode 100644 index 0000000..ff24db3 --- /dev/null +++ b/engineer2_arm_config/launch/planning_context.launch @@ -0,0 +1,25 @@ + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/engineer2_arm_config/launch/planning_pipeline.launch.xml b/engineer2_arm_config/launch/planning_pipeline.launch.xml new file mode 100644 index 0000000..6882ffa --- /dev/null +++ b/engineer2_arm_config/launch/planning_pipeline.launch.xml @@ -0,0 +1,10 @@ + + + + + + + + + diff --git a/engineer2_arm_config/launch/rviz.launch b/engineer2_arm_config/launch/rviz.launch new file mode 100644 index 0000000..2c217fe --- /dev/null +++ b/engineer2_arm_config/launch/rviz.launch @@ -0,0 +1,14 @@ + + + + + + + + + + + + + diff --git a/engineer2_arm_config/launch/servo.launch b/engineer2_arm_config/launch/servo.launch new file mode 100644 index 0000000..d874e7b --- /dev/null +++ b/engineer2_arm_config/launch/servo.launch @@ -0,0 +1,6 @@ + + + + + + diff --git a/engineer2_arm_config/launch/trajectory_execution.launch.xml b/engineer2_arm_config/launch/trajectory_execution.launch.xml new file mode 100644 index 0000000..1801408 --- /dev/null +++ b/engineer2_arm_config/launch/trajectory_execution.launch.xml @@ -0,0 +1,21 @@ + + + + + + + + + + + + + + + + + + + + diff --git a/engineer2_arm_config/package.xml b/engineer2_arm_config/package.xml new file mode 100644 index 0000000..3cc0444 --- /dev/null +++ b/engineer2_arm_config/package.xml @@ -0,0 +1,27 @@ + + engineer2_arm_config + 1.0.0 + + An automatically generated package with all the configuration and launch files for using the engineer with the + MoveIt Motion Planning Framework + + QiayuanLiao + QiayuanLiao + + BSD + + http://moveit.ros.org/ + https://github.com/ros-planning/moveit/issues + https://github.com/ros-planning/moveit + + catkin + + moveit_ros_move_group + moveit_fake_controller_manager + moveit_kinematics + moveit_planners_ompl + moveit_ros_visualization + moveit_setup_assistant + tf2_ros + xacro + diff --git a/engineer_middleware/config/engineer2.yaml b/engineer_middleware/config/engineer2.yaml new file mode 100644 index 0000000..24d6f86 --- /dev/null +++ b/engineer_middleware/config/engineer2.yaml @@ -0,0 +1,9 @@ +chassis: + x: + 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: 4.5, i: 0.0, d: 0.2, i_clamp_max: 0, i_clamp_min: 0, antiwindup: true, publish_state: true } + yaw: + pid: { p: 5., i: 0.0, d: 0.0, i_clamp_max: 0, i_clamp_min: 0, antiwindup: true, publish_state: true } + yaw_start_threshold: 0.05 + max_vel: 10 diff --git a/engineer_middleware/config/engineer2_steps_list.yaml b/engineer_middleware/config/engineer2_steps_list.yaml new file mode 100644 index 0000000..e69de29 diff --git a/engineer_middleware/config/steps_list.yaml b/engineer_middleware/config/engineer_steps_list.yaml similarity index 100% rename from engineer_middleware/config/steps_list.yaml rename to engineer_middleware/config/engineer_steps_list.yaml diff --git a/engineer_middleware/launch/load.launch b/engineer_middleware/launch/load.launch index 778c77c..816a8c9 100644 --- a/engineer_middleware/launch/load.launch +++ b/engineer_middleware/launch/load.launch @@ -1,7 +1,8 @@ - + - diff --git a/engineer_middleware/launch/sim.launch b/engineer_middleware/launch/sim.launch index 394f0ff..7eebf7e 100644 --- a/engineer_middleware/launch/sim.launch +++ b/engineer_middleware/launch/sim.launch @@ -1,10 +1,11 @@ + - - - + + + From 85838176eee47efd34ed046b46faac8ff2b88a22 Mon Sep 17 00:00:00 2001 From: cc0h <7921166012@qq.com> Date: Mon, 1 Jul 2024 17:41:54 +0800 Subject: [PATCH 37/80] Modify Lv5 exchange pos of engineer. --- engineer_middleware/config/engineer_steps_list.yaml | 8 ++++---- engineer_middleware/include/engineer_middleware/motion.h | 4 ++-- 2 files changed, 6 insertions(+), 6 deletions(-) diff --git a/engineer_middleware/config/engineer_steps_list.yaml b/engineer_middleware/config/engineer_steps_list.yaml index cd2f7a9..2a50cb9 100644 --- a/engineer_middleware/config/engineer_steps_list.yaml +++ b/engineer_middleware/config/engineer_steps_list.yaml @@ -1284,7 +1284,7 @@ steps_list: change: "-1" - step: "exchange pos" arm: - joints: [*JOINT1_DOWN_POSITION, *JOINT2_EXCHANGE_POSITION, *JOINT3_MID_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_L90_POSITION, *JOINT6_UP_POSITION, *JOINT7_MID_POSITION] common: <<: *NORMALLY tolerance: @@ -1340,7 +1340,7 @@ steps_list: <<: *NORMAL_TOLERANCE - step: "exchange pos" arm: - joints: [*JOINT1_DOWN_POSITION, *JOINT2_EXCHANGE_POSITION, *JOINT3_MID_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_R90_POSITION, *JOINT6_UP_POSITION, *JOINT7_MID_POSITION] common: <<: *NORMALLY tolerance: @@ -1399,7 +1399,7 @@ steps_list: <<: *NORMAL_TOLERANCE - step: "exchange pos" arm: - joints: [*JOINT1_DOWN_POSITION, *JOINT2_EXCHANGE_POSITION, *JOINT3_MID_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_L90_POSITION, *JOINT6_UP_POSITION, *JOINT7_MID_POSITION] common: <<: *NORMALLY tolerance: @@ -1458,7 +1458,7 @@ steps_list: <<: *NORMAL_TOLERANCE - step: "exchange pos" arm: - joints: [*JOINT1_DOWN_POSITION, *JOINT2_EXCHANGE_POSITION, *JOINT3_MID_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_R90_POSITION, *JOINT6_UP_POSITION, *JOINT7_MID_POSITION] common: <<: *NORMALLY tolerance: diff --git a/engineer_middleware/include/engineer_middleware/motion.h b/engineer_middleware/include/engineer_middleware/motion.h index 3ad3ff1..06f19fb 100644 --- a/engineer_middleware/include/engineer_middleware/motion.h +++ b/engineer_middleware/include/engineer_middleware/motion.h @@ -460,8 +460,8 @@ class JointMotion : public MoveitMotionBase { error = std::abs(final_target_[i] - current[i]); joint_reached = (error < tolerance_joints_[i]); - if (!joint_reached) - ROS_INFO_STREAM("Joint" << i + 1 << " didn't reach configured tolerance range,error: " << error); + // if (!joint_reached) + // ROS_INFO_STREAM("Joint" << i + 1 << " didn't reach configured tolerance range,error: " << error); flag &= joint_reached; } return flag; From 69188ab50d4386968b1aa09d13bc5149c0351578 Mon Sep 17 00:00:00 2001 From: cc0h <7921166012@qq.com> Date: Sun, 7 Jul 2024 17:53:15 +0800 Subject: [PATCH 38/80] Record engineer2 arm config pack.Can be used to run gazebo and moveit. --- engineer2_arm_config/.setup_assistant | 6 +- engineer2_arm_config/CMakeLists.txt | 2 +- .../config/cartesian_limits.yaml | 5 + .../config/chomp_planning.yaml | 6 +- engineer2_arm_config/config/engineer2.srdf | 51 +- .../config/fake_controllers.yaml | 4 +- .../config/gazebo_controllers.yaml | 4 + .../config/gazebo_engineer2.urdf | 2628 +++++++++++++++++ engineer2_arm_config/config/joint_limits.yaml | 36 +- engineer2_arm_config/config/kinematics.yaml | 4 + .../config/ompl_planning.yaml | 52 +- .../config/ros_controllers.yaml | 57 +- engineer2_arm_config/config/sensors_3d.yaml | 1 - .../config/simple_moveit_controllers.yaml | 12 + .../config/stomp_planning.yaml | 39 + .../launch/chomp_planning_pipeline.launch.xml | 26 +- ...eer2_moveit_controller_manager.launch.xml} | 0 ...ngineer2_moveit_sensor_manager.launch.xml} | 0 .../fake_moveit_controller_manager.launch.xml | 5 +- engineer2_arm_config/launch/gazebo.launch | 34 + .../launch/joystick_control.launch | 17 + .../launch/load_controllers.launch | 22 +- engineer2_arm_config/launch/move_group.launch | 145 +- engineer2_arm_config/launch/moveit.rviz | 451 +-- .../launch/moveit_rviz.launch | 15 + .../launch/ompl_planning_pipeline.launch.xml | 35 +- .../launch/planning_context.launch | 35 +- .../launch/planning_pipeline.launch.xml | 8 +- ...ntrol_moveit_controller_manager.launch.xml | 4 + .../launch/ros_controllers.launch | 11 + .../launch/sensor_manager.launch.xml | 17 + .../launch/setup_assistant.launch | 16 + ...imple_moveit_controller_manager.launch.xml | 8 + .../launch/stomp_planning_pipeline.launch.xml | 23 + .../launch/trajectory_execution.launch.xml | 30 +- engineer2_arm_config/package.xml | 56 +- .../launch/load_controllers.launch | 2 +- .../engineer2_simulation_controllers.yaml | 31 + .../config/engineer2_steps_list.yaml | 24 + ...l => engineer_simulation_controllers.yaml} | 0 40 files changed, 3234 insertions(+), 688 deletions(-) create mode 100644 engineer2_arm_config/config/cartesian_limits.yaml create mode 100644 engineer2_arm_config/config/gazebo_controllers.yaml create mode 100644 engineer2_arm_config/config/gazebo_engineer2.urdf create mode 100644 engineer2_arm_config/config/simple_moveit_controllers.yaml create mode 100644 engineer2_arm_config/config/stomp_planning.yaml rename engineer2_arm_config/launch/{engineer_moveit_controller_manager.launch.xml => engineer2_moveit_controller_manager.launch.xml} (100%) rename engineer2_arm_config/launch/{engineer_moveit_sensor_manager.launch.xml => engineer2_moveit_sensor_manager.launch.xml} (100%) create mode 100644 engineer2_arm_config/launch/gazebo.launch create mode 100644 engineer2_arm_config/launch/joystick_control.launch create mode 100644 engineer2_arm_config/launch/moveit_rviz.launch create mode 100644 engineer2_arm_config/launch/ros_control_moveit_controller_manager.launch.xml create mode 100644 engineer2_arm_config/launch/ros_controllers.launch create mode 100644 engineer2_arm_config/launch/sensor_manager.launch.xml create mode 100644 engineer2_arm_config/launch/setup_assistant.launch create mode 100644 engineer2_arm_config/launch/simple_moveit_controller_manager.launch.xml create mode 100644 engineer2_arm_config/launch/stomp_planning_pipeline.launch.xml create mode 100644 engineer_middleware/config/engineer2_simulation_controllers.yaml rename engineer_middleware/config/{simulation_controllers.yaml => engineer_simulation_controllers.yaml} (100%) diff --git a/engineer2_arm_config/.setup_assistant b/engineer2_arm_config/.setup_assistant index 197025b..7ab7a69 100644 --- a/engineer2_arm_config/.setup_assistant +++ b/engineer2_arm_config/.setup_assistant @@ -6,6 +6,6 @@ moveit_setup_assistant_config: SRDF: relative_path: config/engineer2.srdf CONFIG: - author_name: QiayuanLiao - author_email: liaoqiayuan@gmail.com - generated_timestamp: 1617294211 + author_name: Chonghong Cai + author_email: 792166012@qq.com + generated_timestamp: 1720344342 diff --git a/engineer2_arm_config/CMakeLists.txt b/engineer2_arm_config/CMakeLists.txt index ad88885..3121434 100644 --- a/engineer2_arm_config/CMakeLists.txt +++ b/engineer2_arm_config/CMakeLists.txt @@ -6,5 +6,5 @@ find_package(catkin REQUIRED) catkin_package() install(DIRECTORY launch DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} - PATTERN "setup_assistant.launch" EXCLUDE) + PATTERN "setup_assistant.launch" EXCLUDE) install(DIRECTORY config DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}) diff --git a/engineer2_arm_config/config/cartesian_limits.yaml b/engineer2_arm_config/config/cartesian_limits.yaml new file mode 100644 index 0000000..7df72f6 --- /dev/null +++ b/engineer2_arm_config/config/cartesian_limits.yaml @@ -0,0 +1,5 @@ +cartesian_limits: + max_trans_vel: 1 + max_trans_acc: 2.25 + max_trans_dec: -5 + max_rot_vel: 1.57 diff --git a/engineer2_arm_config/config/chomp_planning.yaml b/engineer2_arm_config/config/chomp_planning.yaml index 0831fda..eb9c912 100644 --- a/engineer2_arm_config/config/chomp_planning.yaml +++ b/engineer2_arm_config/config/chomp_planning.yaml @@ -7,12 +7,12 @@ learning_rate: 0.01 smoothness_cost_velocity: 0.0 smoothness_cost_acceleration: 1.0 smoothness_cost_jerk: 0.0 -ridge_factor: 0.01 +ridge_factor: 0.0 use_pseudo_inverse: false pseudo_inverse_ridge_factor: 1e-4 joint_update_limit: 0.1 collision_clearance: 0.2 collision_threshold: 0.07 use_stochastic_descent: true -enable_failure_recovery: true -max_recovery_attempts: 15 +enable_failure_recovery: false +max_recovery_attempts: 5 diff --git a/engineer2_arm_config/config/engineer2.srdf b/engineer2_arm_config/config/engineer2.srdf index a9012bb..1c6d7cc 100644 --- a/engineer2_arm_config/config/engineer2.srdf +++ b/engineer2_arm_config/config/engineer2.srdf @@ -3,53 +3,34 @@ This is a format for representing semantic information about the robot structure. A URDF file must exist for this robot as well, where the joints and the links that are referenced are defined --> - + - - - - - - + - + - + - - - - - - - - - - - - - - - - - - - - - - - - - - + + + + + + + + + + + + diff --git a/engineer2_arm_config/config/fake_controllers.yaml b/engineer2_arm_config/config/fake_controllers.yaml index c51e46f..4167cc8 100644 --- a/engineer2_arm_config/config/fake_controllers.yaml +++ b/engineer2_arm_config/config/fake_controllers.yaml @@ -9,5 +9,5 @@ controller_list: - joint5 - joint6 initial: # Define initial robot poses per group -# - group: engineer_arm -# pose: home + - group: engineer_arm + pose: home diff --git a/engineer2_arm_config/config/gazebo_controllers.yaml b/engineer2_arm_config/config/gazebo_controllers.yaml new file mode 100644 index 0000000..e4d2eb0 --- /dev/null +++ b/engineer2_arm_config/config/gazebo_controllers.yaml @@ -0,0 +1,4 @@ +# Publish joint_states +joint_state_controller: + type: joint_state_controller/JointStateController + publish_rate: 50 diff --git a/engineer2_arm_config/config/gazebo_engineer2.urdf b/engineer2_arm_config/config/gazebo_engineer2.urdf new file mode 100644 index 0000000..e04abdc --- /dev/null +++ b/engineer2_arm_config/config/gazebo_engineer2.urdf @@ -0,0 +1,2628 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + Gazebo/Black + + / + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + Gazebo/Black + + + + + + + + + + + + + + + + + + + + + + + + + + + + + Gazebo/Black + + + + + + + + + + + + + + + + + + + + + + + + + + + + + Gazebo/Black + + + + + + + + + + + + + + + + + + + + + + + + + + + + + Gazebo/Black + + + + + + + + + + + + + + + + + + + + + + + + + + + + + Gazebo/Black + + + + + + + + + + + + + + + + + + + + + + + + + + + + + Gazebo/Black + + + + + + + + + + + + + + + + + + + + + + + + + + + + + Gazebo/Black + + + + + + + + + + + + + + + + + + + + + + + + + + + + + Gazebo/Black + + + + + + + + + + + + + + + + + + + + + + + + + + + + + Gazebo/Black + + + + + + + + + + + + + + + + + + + + + + + + + + + + + Gazebo/Black + + + + + + + + + + + + + + + + + + + + + + + + + + + + + Gazebo/Black + + + + + + + + + + + + + + + + + + + + + + + + + + + + + Gazebo/Black + + + + + + + + + + + + + + + + + + + + + + + + + + + + + Gazebo/Black + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + Gazebo/Black + + + + + + + + + + + + + + + + + + + + + + + + + + + + + Gazebo/Black + + + + + + + + + + + + + + + + + + + + + + + + + + + + + Gazebo/Black + + + + + + + + + + + + + + + + + + + + + + + + + + + + + Gazebo/Black + + + + + + + + + + + + + + + + + + + + + + + + + + + + + Gazebo/Black + + + + + + + + + + + + + + + + + + + + + + + + + + + + + Gazebo/Black + + + + + + + + + + + + + + + + + + + + + + + + + + + + + Gazebo/Black + + + + + + + + + + + + + + + + + + + + + + + + + + + + + Gazebo/Black + + + + + + + + + + + + + + + + + + + + + + + + + + + + + Gazebo/Black + + + + + + + + + + + + + + + + + + + + + + + + + + + + + Gazebo/Black + + + + + + + + + + + + + + + + + + + + + + + + + + + + + Gazebo/Black + + + + + + + + + + + + + + + + + + + + + + + + + + + + + Gazebo/Black + + + + + + + + + + + + + + + + + + + + + + + + + + + + + Gazebo/Black + + + + + + + + + + + + + + + + + + + + + + + + + + + + + Gazebo/Black + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + Gazebo/Black + + + + + + + + + + + + + + + + + + + + + + + + + + + + + Gazebo/Black + + + + + + + + + + + + + + + + + + + + + + + + + + + + + Gazebo/Black + + + + + + + + + + + + + + + + + + + + + + + + + + + + + Gazebo/Black + + + + + + + + + + + + + + + + + + + + + + + + + + + + + Gazebo/Black + + + + + + + + + + + + + + + + + + + + + + + + + + + + + Gazebo/Black + + + + + + + + + + + + + + + + + + + + + + + + + + + + + Gazebo/Black + + + + + + + + + + + + + + + + + + + + + + + + + + + + + Gazebo/Black + + + + + + + + + + + + + + + + + + + + + + + + + + + + + Gazebo/Black + + + + + + + + + + + + + + + + + + + + + + + + + + + + + Gazebo/Black + + + + + + + + + + + + + + + + + + + + + + + + + + + + + Gazebo/Black + + + + + + + + + + + + + + + + + + + + + + + + + + + + + Gazebo/Black + + + + + + + + + + + + + + + + + + + + + + + + + + + + + Gazebo/Black + + + + + + + + + + + + + + + + + + + + + + + + + + + + + Gazebo/Black + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + Gazebo/Black + + + + + + + + + + + + + + + + + + + + + + + + + + + + + Gazebo/Black + + + + + + + + + + + + + + + + + + + + + + + + + + + + + Gazebo/Black + + + + + + + + + + + + + + + + + + + + + + + + + + + + + Gazebo/Black + + + + + + + + + + + + + + + + + + + + + + + + + + + + + Gazebo/Black + + + + + + + + + + + + + + + + + + + + + + + + + + + + + Gazebo/Black + + + + + + + + + + + + + + + + + + + + + + + + + + + + + Gazebo/Black + + + + + + + + + + + + + + + + + + + + + + + + + + + + + Gazebo/Black + + + + + + + + + + + + + + + + + + + + + + + + + + + + + Gazebo/Black + + + + + + + + + + + + + + + + + + + + + + + + + + + + + Gazebo/Black + + + + + + + + + + + + + + + + + + + + + + + + + + + + + Gazebo/Black + + + + + + + + + + + + + + + + + + + + + + + + + + + + + Gazebo/Black + + + + + + + + + + + + + + + + + + + + + + + + + + + + + Gazebo/Black + + + + + + + + + + + + + + + + + + + + + + + + + + + + + Gazebo/Black + + + transmission_interface/SimpleTransmission + + hardware_interface/EffortJointInterface + 19.2032 + + + hardware_interface/EffortJointInterface + + + + transmission_interface/SimpleTransmission + + hardware_interface/EffortJointInterface + -19.2032 + + + hardware_interface/EffortJointInterface + + + + transmission_interface/SimpleTransmission + + hardware_interface/EffortJointInterface + 19.2032 + + + hardware_interface/EffortJointInterface + + + + transmission_interface/SimpleTransmission + + hardware_interface/EffortJointInterface + -19.2032 + + + hardware_interface/EffortJointInterface + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + transmission_interface/SimpleTransmission + + 421.49 + hardware_interface/EffortJointInterface + + + hardware_interface/EffortJointInterface + 0. + + + + transmission_interface/SimpleTransmission + + 1 + hardware_interface/EffortJointInterface + + + hardware_interface/EffortJointInterface + 0. + + + + transmission_interface/SimpleTransmission + + 1 + hardware_interface/EffortJointInterface + + + hardware_interface/EffortJointInterface + 0 + + + + transmission_interface/SimpleTransmission + + 1 + hardware_interface/EffortJointInterface + + + hardware_interface/EffortJointInterface + 0 + + + + transmission_interface/DifferentialTransmission + + actuator1 + 1 + + + actuator2 + 1 + + + hardware_interface/EffortJointInterface + 0 + joint1 + + + hardware_interface/EffortJointInterface + 0 + joint2 + + + + + + + + + + + + transmission_interface/SimpleTransmission + + hardware_interface/EffortJointInterface + + + hardware_interface/EffortJointInterface + 1 + + + + transmission_interface/SimpleTransmission + + hardware_interface/EffortJointInterface + + + hardware_interface/EffortJointInterface + 1 + + + + transmission_interface/SimpleTransmission + + hardware_interface/EffortJointInterface + + + hardware_interface/EffortJointInterface + 1 + + + + transmission_interface/SimpleTransmission + + hardware_interface/EffortJointInterface + + + hardware_interface/EffortJointInterface + 1 + + + + transmission_interface/SimpleTransmission + + hardware_interface/EffortJointInterface + + + hardware_interface/EffortJointInterface + 1 + + + + transmission_interface/SimpleTransmission + + hardware_interface/EffortJointInterface + + + hardware_interface/EffortJointInterface + 1 + + + + transmission_interface/SimpleTransmission + + hardware_interface/EffortJointInterface + + + hardware_interface/EffortJointInterface + 1 + + + + transmission_interface/SimpleTransmission + + hardware_interface/EffortJointInterface + + + hardware_interface/EffortJointInterface + 1 + + + + transmission_interface/SimpleTransmission + + hardware_interface/EffortJointInterface + + + hardware_interface/EffortJointInterface + 1 + + + + transmission_interface/SimpleTransmission + + hardware_interface/EffortJointInterface + + + hardware_interface/EffortJointInterface + 1 + + + + transmission_interface/SimpleTransmission + + hardware_interface/EffortJointInterface + + + hardware_interface/EffortJointInterface + 1 + + + + transmission_interface/SimpleTransmission + + hardware_interface/EffortJointInterface + + + hardware_interface/EffortJointInterface + 1 + + + + transmission_interface/SimpleTransmission + + hardware_interface/EffortJointInterface + + + hardware_interface/EffortJointInterface + 1 + + + + transmission_interface/SimpleTransmission + + hardware_interface/EffortJointInterface + + + hardware_interface/EffortJointInterface + 1 + + + + transmission_interface/SimpleTransmission + + hardware_interface/EffortJointInterface + + + hardware_interface/EffortJointInterface + 1 + + + + transmission_interface/SimpleTransmission + + hardware_interface/EffortJointInterface + + + hardware_interface/EffortJointInterface + 1 + + + + transmission_interface/SimpleTransmission + + hardware_interface/EffortJointInterface + + + hardware_interface/EffortJointInterface + 1 + + + + transmission_interface/SimpleTransmission + + hardware_interface/EffortJointInterface + + + hardware_interface/EffortJointInterface + 1 + + + + transmission_interface/SimpleTransmission + + hardware_interface/EffortJointInterface + + + hardware_interface/EffortJointInterface + 1 + + + + transmission_interface/SimpleTransmission + + hardware_interface/EffortJointInterface + + + hardware_interface/EffortJointInterface + 1 + + + + transmission_interface/SimpleTransmission + + hardware_interface/EffortJointInterface + + + hardware_interface/EffortJointInterface + 1 + + + + transmission_interface/SimpleTransmission + + hardware_interface/EffortJointInterface + + + hardware_interface/EffortJointInterface + 1 + + + + transmission_interface/SimpleTransmission + + hardware_interface/EffortJointInterface + + + hardware_interface/EffortJointInterface + 1 + + + + transmission_interface/SimpleTransmission + + hardware_interface/EffortJointInterface + + + hardware_interface/EffortJointInterface + 1 + + + + transmission_interface/SimpleTransmission + + hardware_interface/EffortJointInterface + + + hardware_interface/EffortJointInterface + 1 + + + + transmission_interface/SimpleTransmission + + hardware_interface/EffortJointInterface + + + hardware_interface/EffortJointInterface + 1 + + + + transmission_interface/SimpleTransmission + + hardware_interface/EffortJointInterface + + + hardware_interface/EffortJointInterface + 1 + + + + transmission_interface/SimpleTransmission + + hardware_interface/EffortJointInterface + + + hardware_interface/EffortJointInterface + 1 + + + + transmission_interface/SimpleTransmission + + hardware_interface/EffortJointInterface + + + hardware_interface/EffortJointInterface + 1 + + + + transmission_interface/SimpleTransmission + + hardware_interface/EffortJointInterface + + + hardware_interface/EffortJointInterface + 1 + + + + transmission_interface/SimpleTransmission + + hardware_interface/EffortJointInterface + + + hardware_interface/EffortJointInterface + 1 + + + + transmission_interface/SimpleTransmission + + hardware_interface/EffortJointInterface + + + hardware_interface/EffortJointInterface + 1 + + + + transmission_interface/SimpleTransmission + + hardware_interface/EffortJointInterface + + + hardware_interface/EffortJointInterface + 1 + + + + transmission_interface/SimpleTransmission + + hardware_interface/EffortJointInterface + + + hardware_interface/EffortJointInterface + 1 + + + + transmission_interface/SimpleTransmission + + hardware_interface/EffortJointInterface + + + hardware_interface/EffortJointInterface + 1 + + + + transmission_interface/SimpleTransmission + + hardware_interface/EffortJointInterface + + + hardware_interface/EffortJointInterface + 1 + + + + transmission_interface/SimpleTransmission + + hardware_interface/EffortJointInterface + + + hardware_interface/EffortJointInterface + 1 + + + + transmission_interface/SimpleTransmission + + hardware_interface/EffortJointInterface + + + hardware_interface/EffortJointInterface + 1 + + + + transmission_interface/SimpleTransmission + + hardware_interface/EffortJointInterface + + + hardware_interface/EffortJointInterface + 1 + + + + transmission_interface/SimpleTransmission + + hardware_interface/EffortJointInterface + + + hardware_interface/EffortJointInterface + 1 + + + + transmission_interface/SimpleTransmission + + hardware_interface/EffortJointInterface + + + hardware_interface/EffortJointInterface + 1 + + + + transmission_interface/SimpleTransmission + + hardware_interface/EffortJointInterface + + + hardware_interface/EffortJointInterface + 1 + + + + transmission_interface/SimpleTransmission + + hardware_interface/EffortJointInterface + + + hardware_interface/EffortJointInterface + 1 + + + + transmission_interface/SimpleTransmission + + hardware_interface/EffortJointInterface + + + hardware_interface/EffortJointInterface + 1 + + + + transmission_interface/SimpleTransmission + + hardware_interface/EffortJointInterface + + + hardware_interface/EffortJointInterface + 1 + + + + transmission_interface/SimpleTransmission + + hardware_interface/EffortJointInterface + + + hardware_interface/EffortJointInterface + 1 + + + + transmission_interface/SimpleTransmission + + hardware_interface/EffortJointInterface + + + hardware_interface/EffortJointInterface + 1 + + + + transmission_interface/SimpleTransmission + + hardware_interface/EffortJointInterface + + + hardware_interface/EffortJointInterface + 1 + + + + transmission_interface/SimpleTransmission + + hardware_interface/EffortJointInterface + + + hardware_interface/EffortJointInterface + 1 + + + + transmission_interface/SimpleTransmission + + hardware_interface/EffortJointInterface + + + hardware_interface/EffortJointInterface + 1 + + + + transmission_interface/SimpleTransmission + + hardware_interface/EffortJointInterface + + + hardware_interface/EffortJointInterface + 1 + + + + transmission_interface/SimpleTransmission + + hardware_interface/EffortJointInterface + + + hardware_interface/EffortJointInterface + 1 + + + + transmission_interface/SimpleTransmission + + hardware_interface/EffortJointInterface + + + hardware_interface/EffortJointInterface + 1 + + + + transmission_interface/SimpleTransmission + + hardware_interface/EffortJointInterface + + + hardware_interface/EffortJointInterface + 1 + + + + transmission_interface/SimpleTransmission + + hardware_interface/EffortJointInterface + + + hardware_interface/EffortJointInterface + 1 + + + + transmission_interface/SimpleTransmission + + hardware_interface/EffortJointInterface + + + hardware_interface/EffortJointInterface + 1 + + + + transmission_interface/SimpleTransmission + + hardware_interface/EffortJointInterface + + + hardware_interface/EffortJointInterface + 1 + + + + transmission_interface/SimpleTransmission + + hardware_interface/EffortJointInterface + + + hardware_interface/EffortJointInterface + 1 + + + + transmission_interface/SimpleTransmission + + hardware_interface/EffortJointInterface + + + hardware_interface/EffortJointInterface + 1 + + + diff --git a/engineer2_arm_config/config/joint_limits.yaml b/engineer2_arm_config/config/joint_limits.yaml index f0cf0d3..6699247 100644 --- a/engineer2_arm_config/config/joint_limits.yaml +++ b/engineer2_arm_config/config/joint_limits.yaml @@ -10,31 +10,31 @@ default_acceleration_scaling_factor: 0.1 joint_limits: joint1: has_velocity_limits: true - max_velocity: 1.80 - has_acceleration_limits: true - max_acceleration: 3.5 + max_velocity: 2.25 + has_acceleration_limits: false + max_acceleration: 0 joint2: has_velocity_limits: true - max_velocity: 2.75 - has_acceleration_limits: true - max_acceleration: 6 + max_velocity: 10.46 + has_acceleration_limits: false + max_acceleration: 0 joint3: has_velocity_limits: true - max_velocity: 1.80 - has_acceleration_limits: true - max_acceleration: 5 + max_velocity: 3.76 + has_acceleration_limits: false + max_acceleration: 0 joint4: has_velocity_limits: true - max_velocity: 19 - has_acceleration_limits: true - max_acceleration: 50 + max_velocity: 20.94 + has_acceleration_limits: false + max_acceleration: 0 joint5: has_velocity_limits: true - max_velocity: 15 - has_acceleration_limits: true - max_acceleration: 20 + max_velocity: 10 + has_acceleration_limits: false + max_acceleration: 0 joint6: has_velocity_limits: true - max_velocity: 50 - has_acceleration_limits: true - max_acceleration: 90 + max_velocity: 10 + has_acceleration_limits: false + max_acceleration: 0 diff --git a/engineer2_arm_config/config/kinematics.yaml b/engineer2_arm_config/config/kinematics.yaml index b298b37..3a127a7 100644 --- a/engineer2_arm_config/config/kinematics.yaml +++ b/engineer2_arm_config/config/kinematics.yaml @@ -1,3 +1,7 @@ engineer_arm: kinematics_solver: trac_ik_kinematics_plugin/TRAC_IKKinematicsPlugin + kinematics_solver_search_resolution: 0.005 kinematics_solver_timeout: 0.005 + goal_joint_tolerance: 0.0001 + goal_position_tolerance: 0.0001 + goal_orientation_tolerance: 0.001 diff --git a/engineer2_arm_config/config/ompl_planning.yaml b/engineer2_arm_config/config/ompl_planning.yaml index 8e69e09..205b9ad 100644 --- a/engineer2_arm_config/config/ompl_planning.yaml +++ b/engineer2_arm_config/config/ompl_planning.yaml @@ -3,8 +3,8 @@ planner_configs: type: geometric::AnytimePathShortening shortcut: true # Attempt to shortcut all new solution paths hybridize: true # Compute hybrid solution trajectories - max_hybrid_paths: 40 # Number of hybrid paths generated per iteration - num_planners: 20 # The number of default planners to use for planning + max_hybrid_paths: 24 # Number of hybrid paths generated per iteration + num_planners: 4 # The number of default planners to use for planning planners: "" # A comma-separated list of planner types (e.g., "PRM,EST,RRTConnect"Optionally, planner parameters can be passed to change the default:"PRM[max_nearest_neighbors=5],EST[goal_bias=.5],RRT[range=10. goal_bias=.1]" SBL: type: geometric::SBL @@ -51,8 +51,8 @@ planner_configs: temp_change_factor: 2.0 # how much to increase or decrease temp. default: 2.0 min_temperature: 10e-10 # lower limit of temp change. default: 10e-10 init_temperature: 10e-6 # initial temperature. default: 10e-6 - frountier_threshold: 0.0 # dist new state to nearest neighbor to disqualify as frontier. default: 0.0 set in setup() - frountierNodeRatio: 0.1 # 1/10, or 1 nonfrontier for every 10 frontier. default: 0.1 + frontier_threshold: 0.0 # dist new state to nearest neighbor to disqualify as frontier. default: 0.0 set in setup() + frontier_node_ratio: 0.1 # 1/10, or 1 nonfrontier for every 10 frontier. default: 0.1 k_constant: 0.0 # value used to normalize expresssion. default: 0.0 set in setup() PRM: type: geometric::PRM @@ -95,8 +95,8 @@ planner_configs: range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() temp_change_factor: 0.1 # how much to increase or decrease temp. default: 0.1 init_temperature: 100 # initial temperature. default: 100 - frountier_threshold: 0.0 # dist new state to nearest neighbor to disqualify as frontier. default: 0.0 set in setup() - frountier_node_ratio: 0.1 # 1/10, or 1 nonfrontier for every 10 frontier. default: 0.1 + frontier_threshold: 0.0 # dist new state to nearest neighbor to disqualify as frontier. default: 0.0 set in setup() + frontier_node_ratio: 0.1 # 1/10, or 1 nonfrontier for every 10 frontier. default: 0.1 cost_threshold: 1e300 # the cost threshold. Any motion cost that is not better will not be expanded. default: inf LBTRRT: type: geometric::LBTRRT @@ -127,6 +127,43 @@ planner_configs: sparse_delta_fraction: 0.25 # delta fraction for connection distance. This value represents the visibility range of sparse samples. default: 0.25 dense_delta_fraction: 0.001 # delta fraction for interface detection. default: 0.001 max_failures: 5000 # maximum consecutive failure limit. default: 5000 + AITstar: + type: geometric::AITstar + use_k_nearest: 1 # whether to use a k-nearest RGG connection model (1) or an r-disc model (0). Default: 1 + rewire_factor: 1.001 # rewire factor of the RGG. Valid values: [1.0:0.01:3.0]. Default: 1.001 + samples_per_batch: 100 # batch size. Valid values: [1:1:1000]. Default: 100 + use_graph_pruning: 1 # enable graph pruning (1) or not (0). Default: 1 + find_approximate_solutions: 0 # track approximate solutions (1) or not (0). Default: 0 + set_max_num_goals: 1 # maximum number of goals sampled from sampleable goal regions. Valid values: [1:1:1000]. Default: 1 + ABITstar: + type: geometric::ABITstar + use_k_nearest: 1 # whether to use a k-nearest RGG connection model (1) or an r-disc model (0). Default: 1 + rewire_factor: 1.001 # rewire factor of the RGG. Valid values: [1.0:0.01:3.0]. Default: 1.001 + samples_per_batch: 100 # batch size. Valid values: [1:1:1000]. Default: 100 + use_graph_pruning: 1 # enable graph pruning (1) or not (0). Default: 1 + prune_threshold_as_fractional_cost_change: 0.1 # fractional change in the solution cost AND problem measure necessary for pruning to occur. Default: 0.1 + delay_rewiring_to_first_solution: 0 # delay (1) or not (0) rewiring until a solution is found. Default: 0 + use_just_in_time_sampling: 0 # delay the generation of samples until they are * necessary. Only works with r-disc connection and path length minimization. Default: 0 + drop_unconnected_samples_on_prune: 0 # drop unconnected samples when pruning, regardless of their heuristic value. Default: 0 + stop_on_each_solution_improvement: 0 # stop the planner each time a solution improvement is found. Useful for debugging. Default: 0 + use_strict_queue_ordering: 0 # sort edges in the queue at the end of the batch (0) or after each rewiring (1). Default: 0 + find_approximate_solutions: 0 # track approximate solutions (1) or not (0). Default: 0 + initial_inflation_factor: 1000000 # inflation factor for the initial search. Valid values: [1.0:0.01:1000000.0]. Default: 1000000 + inflation_scaling_parameter: 10 # scaling parameter for the inflation factor update policy. Valid values: [1.0:0.01:1000000.0]. Default: 0 + truncation_scaling_parameter: 5.0 # scaling parameter for the truncation factor update policy. Valid values: [1.0:0.01:1000000.0]. Default: 0 + BITstar: + type: geometric::BITstar + use_k_nearest: 1 # whether to use a k-nearest RGG connection model (1) or an r-disc model (0). Default: 1 + rewire_factor: 1.001 # rewire factor of the RGG. Valid values: [1.0:0.01:3.0]. Default: 1.001 + samples_per_batch: 100 # batch size. Valid values: [1:1:1000]. Default: 100 + use_graph_pruning: 1 # enable graph pruning (1) or not (0). Default: 1 + prune_threshold_as_fractional_cost_change: 0.1 # fractional change in the solution cost AND problem measure necessary for pruning to occur. Default: 0.1 + delay_rewiring_to_first_solution: 0 # delay (1) or not (0) rewiring until a solution is found. Default: 0 + use_just_in_time_sampling: 0 # delay the generation of samples until they are * necessary. Only works with r-disc connection and path length minimization. Default: 0 + drop_unconnected_samples_on_prune: 0 # drop unconnected samples when pruning, regardless of their heuristic value. Default: 0 + stop_on_each_solution_improvement: 0 # stop the planner each time a solution improvement is found. Useful for debugging. Default: 0 + use_strict_queue_ordering: 0 # sort edges in the queue at the end of the batch (0) or after each rewiring (1). Default: 0 + find_approximate_solutions: 0 # track approximate solutions (1) or not (0). Default: 0 engineer_arm: default_planner_config: RRTConnect planner_configs: @@ -154,5 +191,8 @@ engineer_arm: - LazyPRMstar - SPARS - SPARStwo + - AITstar + - ABITstar + - BITstar projection_evaluator: joints(joint1,joint2) longest_valid_segment_fraction: 0.005 diff --git a/engineer2_arm_config/config/ros_controllers.yaml b/engineer2_arm_config/config/ros_controllers.yaml index a4837dc..1ca5150 100644 --- a/engineer2_arm_config/config/ros_controllers.yaml +++ b/engineer2_arm_config/config/ros_controllers.yaml @@ -1,13 +1,5 @@ -# Simulation settings for using moveit_sim_controllers -moveit_sim_hw_interface: - joint_model_group: engineer_arm - joint_model_group_pose: home -# Settings for ros_control_boilerplate control loop -generic_hw_control_loop: - loop_hz: 300 - cycle_time_error_threshold: 0.01 -# Settings for ros_control hardware interface -hardware_interface: +arm_trajectory_controller: + type: effort_controllers/JointTrajectoryController joints: - joint1 - joint2 @@ -15,17 +7,34 @@ hardware_interface: - joint4 - joint5 - joint6 - sim_control_mode: 1 # 0: position, 1: velocity - -controller_list: - - name: controllers/arm_trajectory_controller - action_ns: follow_joint_trajectory - default: True - type: FollowJointTrajectory - joints: - - joint1 - - joint2 - - joint3 - - joint4 - - joint5 - - joint6 + gains: + joint1: + p: 100 + d: 1 + i: 1 + i_clamp: 1 + joint2: + p: 100 + d: 1 + i: 1 + i_clamp: 1 + joint3: + p: 100 + d: 1 + i: 1 + i_clamp: 1 + joint4: + p: 100 + d: 1 + i: 1 + i_clamp: 1 + joint5: + p: 100 + d: 1 + i: 1 + i_clamp: 1 + joint6: + p: 100 + d: 1 + i: 1 + i_clamp: 1 diff --git a/engineer2_arm_config/config/sensors_3d.yaml b/engineer2_arm_config/config/sensors_3d.yaml index add9f47..b29057e 100644 --- a/engineer2_arm_config/config/sensors_3d.yaml +++ b/engineer2_arm_config/config/sensors_3d.yaml @@ -1,2 +1 @@ sensors: - - { } diff --git a/engineer2_arm_config/config/simple_moveit_controllers.yaml b/engineer2_arm_config/config/simple_moveit_controllers.yaml new file mode 100644 index 0000000..1da75d2 --- /dev/null +++ b/engineer2_arm_config/config/simple_moveit_controllers.yaml @@ -0,0 +1,12 @@ +controller_list: + - name: arm_trajectory_controller + action_ns: follow_joint_trajectory + type: FollowJointTrajectory + default: True + joints: + - joint1 + - joint2 + - joint3 + - joint4 + - joint5 + - joint6 diff --git a/engineer2_arm_config/config/stomp_planning.yaml b/engineer2_arm_config/config/stomp_planning.yaml new file mode 100644 index 0000000..d0e6815 --- /dev/null +++ b/engineer2_arm_config/config/stomp_planning.yaml @@ -0,0 +1,39 @@ +stomp/engineer_arm: + group_name: engineer_arm + optimization: + num_timesteps: 60 + num_iterations: 40 + num_iterations_after_valid: 0 + num_rollouts: 30 + max_rollouts: 30 + initialization_method: 1 # [1 : LINEAR_INTERPOLATION, 2 : CUBIC_POLYNOMIAL, 3 : MININUM_CONTROL_COST] + control_cost_weight: 0.0 + task: + noise_generator: + - class: stomp_moveit/NormalDistributionSampling + stddev: [0.05, 0.05, 0.05, 0.05, 0.05, 0.05] + cost_functions: + - class: stomp_moveit/CollisionCheck + collision_penalty: 1.0 + cost_weight: 1.0 + kernel_window_percentage: 0.2 + longest_valid_joint_move: 0.05 + noisy_filters: + - class: stomp_moveit/JointLimits + lock_start: True + lock_goal: True + - class: stomp_moveit/MultiTrajectoryVisualization + line_width: 0.02 + rgb: [255, 255, 0] + marker_array_topic: stomp_trajectories + marker_namespace: noisy + update_filters: + - class: stomp_moveit/PolynomialSmoother + poly_order: 6 + - class: stomp_moveit/TrajectoryVisualization + line_width: 0.05 + rgb: [0, 191, 255] + error_rgb: [255, 0, 0] + publish_intermediate: True + marker_topic: stomp_trajectory + marker_namespace: optimized diff --git a/engineer2_arm_config/launch/chomp_planning_pipeline.launch.xml b/engineer2_arm_config/launch/chomp_planning_pipeline.launch.xml index 2f60968..cf0de11 100644 --- a/engineer2_arm_config/launch/chomp_planning_pipeline.launch.xml +++ b/engineer2_arm_config/launch/chomp_planning_pipeline.launch.xml @@ -1,23 +1,21 @@ - - - - + + + + /> - - - - + + + + - - - - + diff --git a/engineer2_arm_config/launch/engineer_moveit_controller_manager.launch.xml b/engineer2_arm_config/launch/engineer2_moveit_controller_manager.launch.xml similarity index 100% rename from engineer2_arm_config/launch/engineer_moveit_controller_manager.launch.xml rename to engineer2_arm_config/launch/engineer2_moveit_controller_manager.launch.xml diff --git a/engineer2_arm_config/launch/engineer_moveit_sensor_manager.launch.xml b/engineer2_arm_config/launch/engineer2_moveit_sensor_manager.launch.xml similarity index 100% rename from engineer2_arm_config/launch/engineer_moveit_sensor_manager.launch.xml rename to engineer2_arm_config/launch/engineer2_moveit_sensor_manager.launch.xml diff --git a/engineer2_arm_config/launch/fake_moveit_controller_manager.launch.xml b/engineer2_arm_config/launch/fake_moveit_controller_manager.launch.xml index d6865a4..64a5b70 100644 --- a/engineer2_arm_config/launch/fake_moveit_controller_manager.launch.xml +++ b/engineer2_arm_config/launch/fake_moveit_controller_manager.launch.xml @@ -1,9 +1,12 @@ + + + - + diff --git a/engineer2_arm_config/launch/gazebo.launch b/engineer2_arm_config/launch/gazebo.launch new file mode 100644 index 0000000..0cd31a0 --- /dev/null +++ b/engineer2_arm_config/launch/gazebo.launch @@ -0,0 +1,34 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/engineer2_arm_config/launch/joystick_control.launch b/engineer2_arm_config/launch/joystick_control.launch new file mode 100644 index 0000000..9411f6e --- /dev/null +++ b/engineer2_arm_config/launch/joystick_control.launch @@ -0,0 +1,17 @@ + + + + + + + + + + + + + + + + + diff --git a/engineer2_arm_config/launch/load_controllers.launch b/engineer2_arm_config/launch/load_controllers.launch index 69275b2..d1aab78 100644 --- a/engineer2_arm_config/launch/load_controllers.launch +++ b/engineer2_arm_config/launch/load_controllers.launch @@ -5,10 +5,10 @@ - + - + @@ -20,26 +20,8 @@ args="load controllers/robot_state_controller controllers/joint_state_controller - controllers/joint1_calibration_controller - controllers/joint2_calibration_controller - controllers/joint3_calibration_controller - controllers/joint4_calibration_controller - controllers/joint5_calibration_controller - controllers/joint6_calibration_controller - controllers/joint7_calibration_controller controllers/arm_trajectory_controller controllers/chassis_controller - 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 - controllers/gimbal_lifter_calibration_controller controllers/arm_trajectory_controller " /> diff --git a/engineer2_arm_config/launch/move_group.launch b/engineer2_arm_config/launch/move_group.launch index 3bc6450..ce737e2 100644 --- a/engineer2_arm_config/launch/move_group.launch +++ b/engineer2_arm_config/launch/move_group.launch @@ -1,65 +1,100 @@ - - - - - - - - - - - - - - - - - - - - - - - - - + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + - - - + + + - - - - - + + + - - - - - - - - - - - - - - - - - - + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/engineer2_arm_config/launch/moveit.rviz b/engineer2_arm_config/launch/moveit.rviz index a0a4838..6a876dd 100644 --- a/engineer2_arm_config/launch/moveit.rviz +++ b/engineer2_arm_config/launch/moveit.rviz @@ -5,13 +5,8 @@ Panels: Property Tree Widget: Expanded: - /MotionPlanning1 - - /MotionPlanning1/Planning Request1 - - /MotionPlanning1/Planning Metrics1 - - /MotionPlanning1/Planned Path1 - - /TF1 - - /TF1/Frames1 Splitter Ratio: 0.5 - Tree Height: 315 + Tree Height: 226 - Class: rviz/Help Name: Help - Class: rviz/Views @@ -19,10 +14,6 @@ Panels: - /Current View1 Name: Views Splitter Ratio: 0.5 -Preferences: - PromptSaveOnExit: true -Toolbars: - toolButtonStyle: 2 Visualization Manager: Class: "" Displays: @@ -32,7 +23,7 @@ Visualization Manager: Color: 160; 160; 164 Enabled: true Line Style: - Line Width: 0.029999999329447746 + Line Width: 0.03 Value: Lines Name: Grid Normal Cell Count: 0 @@ -44,177 +35,30 @@ Visualization Manager: Plane Cell Count: 10 Reference Frame: Value: true - - Acceleration_Scaling_Factor: 0.1 - Class: moveit_rviz_plugin/MotionPlanning + - Class: moveit_rviz_plugin/MotionPlanning Enabled: true - Move Group Namespace: "" - MoveIt_Allow_Approximate_IK: false - MoveIt_Allow_External_Program: false - MoveIt_Allow_Replanning: false - MoveIt_Allow_Sensor_Positioning: false - MoveIt_Planning_Attempts: 10 - MoveIt_Planning_Time: 5 - MoveIt_Use_Cartesian_Path: false - MoveIt_Use_Constraint_Aware_IK: false - MoveIt_Workspace: - Center: - X: 0 - Y: 0 - Z: 0 - Size: - X: 2 - Y: 2 - Z: 2 Name: MotionPlanning Planned Path: - Color Enabled: false - Interrupt Display: false - Links: - All Links Enabled: true - Expand Joint Details: false - Expand Link Details: false - Expand Tree: false - Link Tree Style: Links in Alphabetic Order - base_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - camera_link: - Alpha: 1 - Show Axes: false - Show Trail: false - camera_optical_frame: - Alpha: 1 - Show Axes: false - Show Trail: false - extend_arm_back: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - extend_arm_front: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - gimbal_lifter: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - left_back_wheel: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - left_front_wheel: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - link1: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - link2: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - link3: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - link4: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - link5: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - link6: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - link7: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - mimic_link1: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - ore_bin_lifter: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - ore_bin_rotator: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - pitch: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - right_back_wheel: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - right_front_wheel: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - tools_link: - Alpha: 1 - Show Axes: false - Show Trail: false - tools_link_st: - Alpha: 1 - Show Axes: false - Show Trail: false - yaw: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - Loop Animation: false + Links: ~ + Loop Animation: true Robot Alpha: 0.5 - Robot Color: 150; 50; 150 Show Robot Collision: false Show Robot Visual: true Show Trail: false State Display Time: 0.05 s - Trail Step Size: 1 Trajectory Topic: move_group/display_planned_path - Use Sim Time: false Planning Metrics: Payload: 1 Show Joint Torques: false Show Manipulability: false Show Manipulability Index: false Show Weight Limit: false - TextHeight: 0.07999999821186066 Planning Request: Colliding Link Color: 255; 0; 0 Goal State Alpha: 1 Goal State Color: 250; 128; 0 Interactive Marker Size: 0 Joint Violation Color: 255; 0; 255 - Planning Group: engineer_arm Query Goal State: true Query Start State: false Show Workspace: false @@ -224,272 +68,19 @@ Visualization Manager: Robot Description: robot_description Scene Geometry: Scene Alpha: 1 - Scene Color: 50; 230; 50 - Scene Display Time: 0.009999999776482582 Show Scene Geometry: true Voxel Coloring: Z-Axis Voxel Rendering: Occupied Voxels Scene Robot: Attached Body Color: 150; 50; 150 - Links: - All Links Enabled: true - Expand Joint Details: false - Expand Link Details: false - Expand Tree: false - Link Tree Style: Links in Alphabetic Order - base_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - camera_link: - Alpha: 1 - Show Axes: false - Show Trail: false - camera_optical_frame: - Alpha: 1 - Show Axes: false - Show Trail: false - extend_arm_back: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - extend_arm_front: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - gimbal_lifter: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - left_back_wheel: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - left_front_wheel: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - link1: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - link2: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - link3: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - link4: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - link5: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - link6: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - link7: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - mimic_link1: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - ore_bin_lifter: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - ore_bin_rotator: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - pitch: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - right_back_wheel: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - right_front_wheel: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - tools_link: - Alpha: 1 - Show Axes: false - Show Trail: false - tools_link_st: - Alpha: 1 - Show Axes: false - Show Trail: false - yaw: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true + Links: ~ Robot Alpha: 0.5 - Show Robot Collision: false - Show Robot Visual: true - Value: true - Velocity_Scaling_Factor: 0.1 - - Class: rviz/TF - Enabled: true - Filter (blacklist): "" - Filter (whitelist): "" - Frame Timeout: 15 - Frames: - All Enabled: false - base_link: - Value: true - camera_link: - Value: false - camera_optical_frame: - Value: false - exchanger: - Value: true - extend_arm_back: - Value: false - extend_arm_front: - Value: false - gimbal_lifter: - Value: false - left_back_wheel: - Value: false - left_front_wheel: - Value: false - link1: - Value: false - link2: - Value: false - link3: - Value: false - link4: - Value: false - link5: - Value: false - link6: - Value: false - link7: - Value: true - map: - Value: false - mimic_link1: - Value: false - odom: - Value: false - ore_bin_lifter: - Value: false - ore_bin_rotator: - Value: false - pitch: - Value: false - real_world: - Value: false - right_back_wheel: - Value: false - right_front_wheel: - Value: false - tools_link: - Value: false - tools_link_st: - Value: false - yaw: - Value: false - Marker Alpha: 1 - Marker Scale: 1 - Name: TF - Show Arrows: true - Show Axes: true - Show Names: true - Tree: - base_link: - {} - exchanger: - {} - extend_arm_back: - {} - extend_arm_front: - {} - gimbal_lifter: - camera_link: - {} - camera_optical_frame: - {} - left_back_wheel: - {} - left_front_wheel: - {} - link1: - {} - link2: - {} - link3: - {} - link4: - {} - link5: - {} - link6: - {} - link7: - tools_link: - {} - tools_link_st: - {} - map: - {} - mimic_link1: - {} - odom: - {} - ore_bin_lifter: - {} - ore_bin_rotator: - {} - pitch: - {} - real_world: - {} - right_back_wheel: - {} - right_front_wheel: - {} - yaw: - {} - Update Interval: 0 + Show Scene Robot: true Value: true Enabled: true Global Options: Background Color: 48; 48; 48 - Default Light: true Fixed Frame: base_link - Frame Rate: 30 Name: root Tools: - Class: rviz/Interact @@ -500,30 +91,30 @@ Visualization Manager: Views: Current: Class: rviz/Orbit - Distance: 1.4108070135116577 + Distance: 2.0 Enable Stereo Rendering: - Stereo Eye Separation: 0.05999999865889549 + Stereo Eye Separation: 0.06 Stereo Focal Distance: 1 Swap Stereo Eyes: false Value: false Field of View: 0.75 Focal Point: - X: 0.23874804377555847 - Y: 0.04174364358186722 - Z: 0.6224369406700134 + X: -0.1 + Y: 0.25 + Z: 0.30 Focal Shape Fixed Size: true - Focal Shape Size: 0.05000000074505806 + Focal Shape Size: 0.05 Invert Z Axis: false Name: Current View - Near Clip Distance: 0.009999999776482582 - Pitch: 0.48979732394218445 + Near Clip Distance: 0.01 + Pitch: 0.5 Target Frame: base_link - Yaw: 3.2827558517456055 + Yaw: -0.6232355833053589 Saved: ~ Window Geometry: Displays: collapsed: false - Height: 1016 + Height: 848 Help: collapsed: false Hide Left Dock: false @@ -532,9 +123,9 @@ Window Geometry: collapsed: false MotionPlanning - Trajectory Slider: collapsed: false - QMainWindow State: 000000ff00000000fd0000000100000000000001f30000039efc0200000007fb000000100044006900730070006c006100790073010000003d000001cc000000c900fffffffb0000000800480065006c00700000000342000000bb0000006e00fffffffb0000000a00560069006500770073000000010c000000a4000000a400fffffffb0000000c00430061006d00650072006100000002ff000001610000000000000000fb0000001e004d006f00740069006f006e00200050006c0061006e006e0069006e00670100000374000001890000000000000000fb00000044004d006f00740069006f006e0050006c0061006e006e0069006e00670020002d0020005400720061006a006500630074006f0072007900200053006c00690064006500720000000000ffffffff0000001600000016fb0000001c004d006f00740069006f006e0050006c0061006e006e0069006e0067010000020f000001cc0000017d00ffffff000004880000039e00000001000000020000000100000002fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + QMainWindow State: 000000ff00000000fd0000000100000000000001f0000002f6fc0200000007fb000000100044006900730070006c006100790073010000003d00000173000000c900fffffffb0000000800480065006c00700000000342000000bb0000006e00fffffffb0000000a00560069006500770073000000010c000000a4000000a400fffffffb0000000c00430061006d00650072006100000002ff000001610000000000000000fb0000001e004d006f00740069006f006e00200050006c0061006e006e0069006e00670100000374000001890000000000000000fb00000044004d006f00740069006f006e0050006c0061006e006e0069006e00670020002d0020005400720061006a006500630074006f0072007900200053006c00690064006500720000000000ffffffff0000001600000016fb0000001c004d006f00740069006f006e0050006c0061006e006e0069006e006701000001b60000017d0000017d00ffffff00000315000002f600000001000000020000000100000002fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 Views: collapsed: false - Width: 1665 - X: 757 - Y: 27 + Width: 1291 + X: 454 + Y: 25 diff --git a/engineer2_arm_config/launch/moveit_rviz.launch b/engineer2_arm_config/launch/moveit_rviz.launch new file mode 100644 index 0000000..a4605c0 --- /dev/null +++ b/engineer2_arm_config/launch/moveit_rviz.launch @@ -0,0 +1,15 @@ + + + + + + + + + + + + + + diff --git a/engineer2_arm_config/launch/ompl_planning_pipeline.launch.xml b/engineer2_arm_config/launch/ompl_planning_pipeline.launch.xml index c219994..ed05662 100644 --- a/engineer2_arm_config/launch/ompl_planning_pipeline.launch.xml +++ b/engineer2_arm_config/launch/ompl_planning_pipeline.launch.xml @@ -1,25 +1,24 @@ - - + + - - + + - + + + + - - - - - + diff --git a/engineer2_arm_config/launch/planning_context.launch b/engineer2_arm_config/launch/planning_context.launch index ff24db3..e527542 100644 --- a/engineer2_arm_config/launch/planning_context.launch +++ b/engineer2_arm_config/launch/planning_context.launch @@ -1,25 +1,26 @@ - - + + - - + + - - + + - - + + - - - - + + + + + - - - - + + + + + diff --git a/engineer2_arm_config/launch/planning_pipeline.launch.xml b/engineer2_arm_config/launch/planning_pipeline.launch.xml index 6882ffa..4b4d0d6 100644 --- a/engineer2_arm_config/launch/planning_pipeline.launch.xml +++ b/engineer2_arm_config/launch/planning_pipeline.launch.xml @@ -1,10 +1,10 @@ - + - + - + diff --git a/engineer2_arm_config/launch/ros_control_moveit_controller_manager.launch.xml b/engineer2_arm_config/launch/ros_control_moveit_controller_manager.launch.xml new file mode 100644 index 0000000..9ebc91c --- /dev/null +++ b/engineer2_arm_config/launch/ros_control_moveit_controller_manager.launch.xml @@ -0,0 +1,4 @@ + + + + diff --git a/engineer2_arm_config/launch/ros_controllers.launch b/engineer2_arm_config/launch/ros_controllers.launch new file mode 100644 index 0000000..f149647 --- /dev/null +++ b/engineer2_arm_config/launch/ros_controllers.launch @@ -0,0 +1,11 @@ + + + + + + + + + + diff --git a/engineer2_arm_config/launch/sensor_manager.launch.xml b/engineer2_arm_config/launch/sensor_manager.launch.xml new file mode 100644 index 0000000..fb429e6 --- /dev/null +++ b/engineer2_arm_config/launch/sensor_manager.launch.xml @@ -0,0 +1,17 @@ + + + + + + + + + + + + + + + + + diff --git a/engineer2_arm_config/launch/setup_assistant.launch b/engineer2_arm_config/launch/setup_assistant.launch new file mode 100644 index 0000000..6e40bfd --- /dev/null +++ b/engineer2_arm_config/launch/setup_assistant.launch @@ -0,0 +1,16 @@ + + + + + + + + + + + + diff --git a/engineer2_arm_config/launch/simple_moveit_controller_manager.launch.xml b/engineer2_arm_config/launch/simple_moveit_controller_manager.launch.xml new file mode 100644 index 0000000..1df16f6 --- /dev/null +++ b/engineer2_arm_config/launch/simple_moveit_controller_manager.launch.xml @@ -0,0 +1,8 @@ + + + + + + + + diff --git a/engineer2_arm_config/launch/stomp_planning_pipeline.launch.xml b/engineer2_arm_config/launch/stomp_planning_pipeline.launch.xml new file mode 100644 index 0000000..0876e8e --- /dev/null +++ b/engineer2_arm_config/launch/stomp_planning_pipeline.launch.xml @@ -0,0 +1,23 @@ + + + + + + + + + + + + + + + + + diff --git a/engineer2_arm_config/launch/trajectory_execution.launch.xml b/engineer2_arm_config/launch/trajectory_execution.launch.xml index 1801408..20c3dfc 100644 --- a/engineer2_arm_config/launch/trajectory_execution.launch.xml +++ b/engineer2_arm_config/launch/trajectory_execution.launch.xml @@ -1,21 +1,23 @@ + - + + + - - - + + + - - - - - - + + + + + + - - - + + diff --git a/engineer2_arm_config/package.xml b/engineer2_arm_config/package.xml index 3cc0444..8c33136 100644 --- a/engineer2_arm_config/package.xml +++ b/engineer2_arm_config/package.xml @@ -1,27 +1,41 @@ - engineer2_arm_config - 1.0.0 - - An automatically generated package with all the configuration and launch files for using the engineer with the - MoveIt Motion Planning Framework - - QiayuanLiao - QiayuanLiao - BSD + engineer2_arm_config + 0.3.0 + + An automatically generated package with all the configuration and launch files for using the engineer2 with the MoveIt Motion Planning Framework + + Chonghong Cai + Chonghong Cai - http://moveit.ros.org/ - https://github.com/ros-planning/moveit/issues - https://github.com/ros-planning/moveit + BSD + + http://moveit.ros.org/ + https://github.com/ros-planning/moveit/issues + https://github.com/ros-planning/moveit + + catkin + + moveit_ros_move_group + moveit_fake_controller_manager + moveit_kinematics + moveit_planners + moveit_ros_visualization + moveit_setup_assistant + moveit_simple_controller_manager + joint_state_publisher + joint_state_publisher_gui + robot_state_publisher + rviz + tf2_ros + xacro + + + + + + rm_description - catkin - moveit_ros_move_group - moveit_fake_controller_manager - moveit_kinematics - moveit_planners_ompl - moveit_ros_visualization - moveit_setup_assistant - tf2_ros - xacro diff --git a/engineer_arm_config/launch/load_controllers.launch b/engineer_arm_config/launch/load_controllers.launch index 69275b2..39c6842 100644 --- a/engineer_arm_config/launch/load_controllers.launch +++ b/engineer_arm_config/launch/load_controllers.launch @@ -8,7 +8,7 @@ - + diff --git a/engineer_middleware/config/engineer2_simulation_controllers.yaml b/engineer_middleware/config/engineer2_simulation_controllers.yaml new file mode 100644 index 0000000..964f39d --- /dev/null +++ b/engineer_middleware/config/engineer2_simulation_controllers.yaml @@ -0,0 +1,31 @@ +controllers: + joint_state_controller: + type: joint_state_controller/JointStateController + publish_rate: 100 + robot_state_controller: + type: robot_state_controller/RobotStateController + publish_rate: 100 + + arm_trajectory_controller: + type: effort_controllers/JointTrajectoryController + joints: + - joint1 + - joint2 + - joint3 + - joint4 + - joint5 + - joint6 + gains: + joint1: { p: 20000.0, i: 0, d: 400.0, i_clamp_max: 0, i_clamp_min: 0, antiwindup: true, publish_state: true } + joint2: { p: 100000, i: 0, d: 1000.0, i_clamp_max: 0, i_clamp_min: 0, antiwindup: true, publish_state: true } + joint3: { p: 5000.0, i: 0, d: 100.0, i_clamp_max: 0, i_clamp_min: 0, antiwindup: true, publish_state: true } + joint4: { p: 10000.0, i: 0.0, d: 60, i_clamp_max: 0, i_clamp_min: 0.0, antiwindup: true, publish_state: true } + joint5: { p: 46000.0, i: 0, d: 200.0, i_clamp_max: 0, i_clamp_min: 0, antiwindup: true, publish_state: true } + joint6: { p: 20000.0, i: 0, d: 120., i_clamp_max: 0, i_clamp_min: 0, antiwindup: true, publish_state: true } + constraints: + joint1: { trajectory: 0.05, goal: 0.05 } + joint2: { trajectory: 0.05, goal: 0.05 } + joint3: { trajectory: 0.05, goal: 0.05 } + joint4: { trajectory: 0.05, goal: 0.05 } + joint5: { trajectory: 0.05, goal: 0.05 } + joint6: { trajectory: 0.05, goal: 0.05 } diff --git a/engineer_middleware/config/engineer2_steps_list.yaml b/engineer_middleware/config/engineer2_steps_list.yaml index e69de29..debfca0 100644 --- a/engineer_middleware/config/engineer2_steps_list.yaml +++ b/engineer_middleware/config/engineer2_steps_list.yaml @@ -0,0 +1,24 @@ +common: + speed: + slowly: &SLOWLY + speed: 0.15 + accel: 0.1 + timeout: 15. + normally: &NORMALLY + speed: 0.4 + accel: 0.3 + timeout: 3 + quickly: &QUICKLY + speed: 0.6 + accel: 0.6 + timeout: 7. + + tolerance: + small_tolerance: &SMALL_TOLERANCE + tolerance_joints: [ 0.005, 0.008, 0.015, 0.1, 0.1, 0.1, 0.03 ] + normal_tolerance: &NORMAL_TOLERANCE + tolerance_joints: [ 0.012, 0.02, 0.015, 0.1, 0.2, 0.15, 0.07 ] + bigger_tolerance: &BIGGER_TOLERANCE + tolerance_joints: [ 0.015, 0.015, 0.02, 0.35, 0.2, 0.2, 0.1 ] + max_tolerance: &MAX_TOLERANCE + tolerance_joints: [ 0.03, 0.03, 0.03, 0.3, 0.3, 0.3 , 0.3] diff --git a/engineer_middleware/config/simulation_controllers.yaml b/engineer_middleware/config/engineer_simulation_controllers.yaml similarity index 100% rename from engineer_middleware/config/simulation_controllers.yaml rename to engineer_middleware/config/engineer_simulation_controllers.yaml From 7867306b281337baa1e12ae4a2e286830821cf66 Mon Sep 17 00:00:00 2001 From: cc0h <7921166012@qq.com> Date: Mon, 8 Jul 2024 10:31:15 +0800 Subject: [PATCH 39/80] Fix wrong collision check matrix. --- engineer2_arm_config/config/engineer2.srdf | 20 ++++++++++---------- 1 file changed, 10 insertions(+), 10 deletions(-) diff --git a/engineer2_arm_config/config/engineer2.srdf b/engineer2_arm_config/config/engineer2.srdf index 1c6d7cc..12140bf 100644 --- a/engineer2_arm_config/config/engineer2.srdf +++ b/engineer2_arm_config/config/engineer2.srdf @@ -22,15 +22,15 @@ - - - - - - - - - - + + + + + + + + + + From 41f9170570b3d2bc848b5412afb5c37e5b285625 Mon Sep 17 00:00:00 2001 From: cc0h <7921166012@qq.com> Date: Mon, 8 Jul 2024 10:34:40 +0800 Subject: [PATCH 40/80] Fix bug: moveit can't find valid controller when using gazebo. --- .../config/fake_controllers.yaml | 2 +- .../config/ros_controllers.yaml | 57 ++++++-------- .../fake_moveit_controller_manager.launch.xml | 2 +- engineer2_arm_config/launch/gazebo.launch | 2 +- .../launch/joystick_control.launch | 17 ---- .../launch/load_controllers.launch | 1 - engineer2_arm_config/launch/move_group.launch | 78 ++++++------------- .../launch/planning_pipeline.launch.xml | 2 +- .../launch/trajectory_execution.launch.xml | 9 ++- .../engineer2_simulation_controllers.yaml | 50 ++++++++++-- 10 files changed, 101 insertions(+), 119 deletions(-) delete mode 100644 engineer2_arm_config/launch/joystick_control.launch diff --git a/engineer2_arm_config/config/fake_controllers.yaml b/engineer2_arm_config/config/fake_controllers.yaml index 4167cc8..42c4827 100644 --- a/engineer2_arm_config/config/fake_controllers.yaml +++ b/engineer2_arm_config/config/fake_controllers.yaml @@ -1,5 +1,5 @@ controller_list: - - name: fake_engineer_arm_controller + - name: arm_trajectory_controller type: $(arg fake_execution_type) joints: - joint1 diff --git a/engineer2_arm_config/config/ros_controllers.yaml b/engineer2_arm_config/config/ros_controllers.yaml index 1ca5150..a4837dc 100644 --- a/engineer2_arm_config/config/ros_controllers.yaml +++ b/engineer2_arm_config/config/ros_controllers.yaml @@ -1,5 +1,13 @@ -arm_trajectory_controller: - type: effort_controllers/JointTrajectoryController +# Simulation settings for using moveit_sim_controllers +moveit_sim_hw_interface: + joint_model_group: engineer_arm + joint_model_group_pose: home +# Settings for ros_control_boilerplate control loop +generic_hw_control_loop: + loop_hz: 300 + cycle_time_error_threshold: 0.01 +# Settings for ros_control hardware interface +hardware_interface: joints: - joint1 - joint2 @@ -7,34 +15,17 @@ arm_trajectory_controller: - joint4 - joint5 - joint6 - gains: - joint1: - p: 100 - d: 1 - i: 1 - i_clamp: 1 - joint2: - p: 100 - d: 1 - i: 1 - i_clamp: 1 - joint3: - p: 100 - d: 1 - i: 1 - i_clamp: 1 - joint4: - p: 100 - d: 1 - i: 1 - i_clamp: 1 - joint5: - p: 100 - d: 1 - i: 1 - i_clamp: 1 - joint6: - p: 100 - d: 1 - i: 1 - i_clamp: 1 + sim_control_mode: 1 # 0: position, 1: velocity + +controller_list: + - name: controllers/arm_trajectory_controller + action_ns: follow_joint_trajectory + default: True + type: FollowJointTrajectory + joints: + - joint1 + - joint2 + - joint3 + - joint4 + - joint5 + - joint6 diff --git a/engineer2_arm_config/launch/fake_moveit_controller_manager.launch.xml b/engineer2_arm_config/launch/fake_moveit_controller_manager.launch.xml index 64a5b70..4d5bbea 100644 --- a/engineer2_arm_config/launch/fake_moveit_controller_manager.launch.xml +++ b/engineer2_arm_config/launch/fake_moveit_controller_manager.launch.xml @@ -7,6 +7,6 @@ - + diff --git a/engineer2_arm_config/launch/gazebo.launch b/engineer2_arm_config/launch/gazebo.launch index 0cd31a0..9f7e235 100644 --- a/engineer2_arm_config/launch/gazebo.launch +++ b/engineer2_arm_config/launch/gazebo.launch @@ -13,7 +13,7 @@ - + diff --git a/engineer2_arm_config/launch/joystick_control.launch b/engineer2_arm_config/launch/joystick_control.launch deleted file mode 100644 index 9411f6e..0000000 --- a/engineer2_arm_config/launch/joystick_control.launch +++ /dev/null @@ -1,17 +0,0 @@ - - - - - - - - - - - - - - - - - diff --git a/engineer2_arm_config/launch/load_controllers.launch b/engineer2_arm_config/launch/load_controllers.launch index d1aab78..78d5395 100644 --- a/engineer2_arm_config/launch/load_controllers.launch +++ b/engineer2_arm_config/launch/load_controllers.launch @@ -22,7 +22,6 @@ controllers/joint_state_controller controllers/arm_trajectory_controller controllers/chassis_controller - controllers/arm_trajectory_controller " /> diff --git a/engineer2_arm_config/launch/move_group.launch b/engineer2_arm_config/launch/move_group.launch index ce737e2..c8771cc 100644 --- a/engineer2_arm_config/launch/move_group.launch +++ b/engineer2_arm_config/launch/move_group.launch @@ -1,84 +1,52 @@ - - - - + + + + - - - - + + + + + - - - - - - + - - - - - - - - - - - + + + - - - + + + + + - - - - - - - - - - - - - - + - + diff --git a/engineer2_arm_config/launch/planning_pipeline.launch.xml b/engineer2_arm_config/launch/planning_pipeline.launch.xml index 4b4d0d6..0929525 100644 --- a/engineer2_arm_config/launch/planning_pipeline.launch.xml +++ b/engineer2_arm_config/launch/planning_pipeline.launch.xml @@ -5,6 +5,6 @@ - + diff --git a/engineer2_arm_config/launch/trajectory_execution.launch.xml b/engineer2_arm_config/launch/trajectory_execution.launch.xml index 20c3dfc..ae4b5af 100644 --- a/engineer2_arm_config/launch/trajectory_execution.launch.xml +++ b/engineer2_arm_config/launch/trajectory_execution.launch.xml @@ -2,7 +2,7 @@ - + @@ -16,8 +16,9 @@ - - + + + diff --git a/engineer_middleware/config/engineer2_simulation_controllers.yaml b/engineer_middleware/config/engineer2_simulation_controllers.yaml index 964f39d..e04d364 100644 --- a/engineer_middleware/config/engineer2_simulation_controllers.yaml +++ b/engineer_middleware/config/engineer2_simulation_controllers.yaml @@ -17,11 +17,11 @@ controllers: - joint6 gains: joint1: { p: 20000.0, i: 0, d: 400.0, i_clamp_max: 0, i_clamp_min: 0, antiwindup: true, publish_state: true } - joint2: { p: 100000, i: 0, d: 1000.0, i_clamp_max: 0, i_clamp_min: 0, antiwindup: true, publish_state: true } - joint3: { p: 5000.0, i: 0, d: 100.0, i_clamp_max: 0, i_clamp_min: 0, antiwindup: true, publish_state: true } - joint4: { p: 10000.0, i: 0.0, d: 60, i_clamp_max: 0, i_clamp_min: 0.0, antiwindup: true, publish_state: true } - joint5: { p: 46000.0, i: 0, d: 200.0, i_clamp_max: 0, i_clamp_min: 0, antiwindup: true, publish_state: true } - joint6: { p: 20000.0, i: 0, d: 120., i_clamp_max: 0, i_clamp_min: 0, antiwindup: true, publish_state: true } + joint2: { p: 100000, i: 0, d: 0.5, i_clamp_max: 0, i_clamp_min: 0, antiwindup: true, publish_state: true } + joint3: { p: 20.0, i: 0, d: 0.1, i_clamp_max: 0, i_clamp_min: 0, antiwindup: true, publish_state: true } + joint4: { p: 20.0, i: 0.0, d: 0.1, i_clamp_max: 0, i_clamp_min: 0.0, antiwindup: true, publish_state: true } + joint5: { p: 20.0, i: 0, d: 0.1, i_clamp_max: 0, i_clamp_min: 0, antiwindup: true, publish_state: true } + joint6: { p: 5.0, i: 0, d: 0.1, i_clamp_max: 0, i_clamp_min: 0, antiwindup: true, publish_state: true } constraints: joint1: { trajectory: 0.05, goal: 0.05 } joint2: { trajectory: 0.05, goal: 0.05 } @@ -29,3 +29,43 @@ controllers: joint4: { trajectory: 0.05, goal: 0.05 } joint5: { trajectory: 0.05, goal: 0.05 } joint6: { trajectory: 0.05, goal: 0.05 } + + chassis_controller: + type: rm_chassis_controllers/OmniController + # ChassisBase + publish_rate: 100 + enable_odom_tf: true + publish_odom_tf: true + power: + effort_coeff: 2.0 + vel_coeff: 0.008 + power_offset: -9.8 + twist_angular: 0.5233 + timeout: 0.1 + pid_follow: { p: 4.0, i: 0, d: 0.02, i_max: 0.0, i_min: 0.0, antiwindup: true, publish_state: true } + twist_covariance_diagonal: [ 0.001, 0.001, 0.001, 0.001, 0.001, 0.001 ] + max_odom_vel: 10.0 + # OmniController + wheels: + left_front: + pose: [ 0.213, 0.1945, 0. ] + roller_angle: -0.785 + joint: left_front_wheel_joint + <<: &wheel_setting + radius: 0.07 + pid: { p: 0.4, i: 0, d: 0.0, i_max: 0.0, i_min: 0.0, antiwindup: true, publish_state: true } + right_front: + pose: [ 0.213, -0.1945, 0. ] + roller_angle: 0.785 + joint: right_front_wheel_joint + <<: *wheel_setting + left_back: + pose: [ -0.213, 0.1945, 0. ] + roller_angle: 0.785 + joint: left_back_wheel_joint + <<: *wheel_setting + right_back: + pose: [ -0.213, -0.1945, 0. ] + roller_angle: -0.785 + joint: right_back_wheel_joint + <<: *wheel_setting From 5b7a75f807924f4e748c76e85fa7349707377de7 Mon Sep 17 00:00:00 2001 From: cc0h <7921166012@qq.com> Date: Wed, 10 Jul 2024 17:50:48 +0800 Subject: [PATCH 41/80] Add controllers for gold and silver structures on engineer2. --- .../launch/load_controllers.launch | 5 +++ .../engineer2_simulation_controllers.yaml | 32 +++++++++++++++++++ 2 files changed, 37 insertions(+) diff --git a/engineer2_arm_config/launch/load_controllers.launch b/engineer2_arm_config/launch/load_controllers.launch index 78d5395..f7015c2 100644 --- a/engineer2_arm_config/launch/load_controllers.launch +++ b/engineer2_arm_config/launch/load_controllers.launch @@ -22,6 +22,11 @@ controllers/joint_state_controller controllers/arm_trajectory_controller controllers/chassis_controller + controllers/silver_lifter_controller + controllers/silver_pusher_controller + controllers/silver_rotator_controller + controllers/gold_lifter_controller + controllers/gold_pusher_controller " /> diff --git a/engineer_middleware/config/engineer2_simulation_controllers.yaml b/engineer_middleware/config/engineer2_simulation_controllers.yaml index e04d364..e41c049 100644 --- a/engineer_middleware/config/engineer2_simulation_controllers.yaml +++ b/engineer_middleware/config/engineer2_simulation_controllers.yaml @@ -69,3 +69,35 @@ controllers: roller_angle: -0.785 joint: right_back_wheel_joint <<: *wheel_setting + + middle_pitch_controller: + type: effort_controllers/JointPositionController + joint: middle_pitch_joint + pid: { p: 5., i: 0., d: 1, i_max: 0.01, i_min: 0.01, antiwindup: true, publish_state: true } + + silver_lifter_controller: + type: effort_controllers/JointPositionController + joint: silver_lifter_joint + pid: { p: 1000., i: 0., d: 100, i_max: 0.01, i_min: 0.01, antiwindup: true, publish_state: true } + + silver_pusher_controller: + type: effort_controllers/JointPositionController + joint: silver_pusher_joint + pid: { p: 1000., i: 0., d: 100, i_max: 0.01, i_min: 0.01, antiwindup: true, publish_state: true } + + silver_rotator_controller: + type: effort_controllers/JointPositionController + joint: silver_rotator_joint + pid: { p: 50., i: 0., d: 1, i_max: 0.01, i_min: 0.01, antiwindup: true, publish_state: true } + + ######################### Gold Ore Controllers ######################## + + gold_lifter_controller: + type: effort_controllers/JointPositionController + joint: gold_lifter_joint + pid: { p: 100., i: 0., d: 10, i_max: 0.01, i_min: 0.01, antiwindup: true, publish_state: true } + + gold_pusher_controller: + type: effort_controllers/JointPositionController + joint: gold_pusher_joint + pid: { p: 1000., i: 0., d: 100, i_max: 0.01, i_min: 0.01, antiwindup: true, publish_state: true } From 5c77a3af9f88f0de540e19410a1b96cc10959444 Mon Sep 17 00:00:00 2001 From: cc0h <7921166012@qq.com> Date: Wed, 10 Jul 2024 17:52:38 +0800 Subject: [PATCH 42/80] Add publishers for get ore structure controllers on engineer2. --- engineer2_arm_config/config/kinematics.yaml | 3 -- .../include/engineer_middleware/middleware.h | 3 +- .../include/engineer_middleware/motion.h | 9 +++- .../include/engineer_middleware/step.h | 42 ++++++++++++++++++- .../include/engineer_middleware/step_queue.h | 7 +++- engineer_middleware/src/middleware.cpp | 17 +++++--- 6 files changed, 68 insertions(+), 13 deletions(-) diff --git a/engineer2_arm_config/config/kinematics.yaml b/engineer2_arm_config/config/kinematics.yaml index 3a127a7..c50afb8 100644 --- a/engineer2_arm_config/config/kinematics.yaml +++ b/engineer2_arm_config/config/kinematics.yaml @@ -2,6 +2,3 @@ engineer_arm: kinematics_solver: trac_ik_kinematics_plugin/TRAC_IKKinematicsPlugin kinematics_solver_search_resolution: 0.005 kinematics_solver_timeout: 0.005 - goal_joint_tolerance: 0.0001 - goal_position_tolerance: 0.0001 - goal_orientation_tolerance: 0.001 diff --git a/engineer_middleware/include/engineer_middleware/middleware.h b/engineer_middleware/include/engineer_middleware/middleware.h index cc9b809..4d89de4 100644 --- a/engineer_middleware/include/engineer_middleware/middleware.h +++ b/engineer_middleware/include/engineer_middleware/middleware.h @@ -83,7 +83,8 @@ class Middleware 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_, extend_arm_f_pub_, - extend_arm_b_pub_; + extend_arm_b_pub_, silver_lifter_pub_, silver_pusher_pub_, silver_rotator_pub_, gold_pusher_pub_, + gold_lifter_pub_, middle_pitch_pub_; std::unordered_map step_queues_; tf2_ros::Buffer tf_; tf2_ros::TransformListener tf_listener_; diff --git a/engineer_middleware/include/engineer_middleware/motion.h b/engineer_middleware/include/engineer_middleware/motion.h index 06f19fb..46024e0 100644 --- a/engineer_middleware/include/engineer_middleware/motion.h +++ b/engineer_middleware/include/engineer_middleware/motion.h @@ -675,15 +675,22 @@ class JointPointMotion : public PublishMotion { ROS_ASSERT(motion.hasMember("target")); target_ = xmlRpcGetDouble(motion, "target", 0.0); + delay_ = xmlRpcGetDouble(motion, "delay", 0.5); } bool move() override { + start_time_ = ros::Time::now(); msg_.data = target_; return PublishMotion::move(); } + bool isFinish() override + { + return ((ros::Time::now() - start_time_).toSec() > delay_); + } private: - double target_; + double target_, delay_; + ros::Time start_time_; }; class ExtendMotion : public PublishMotion diff --git a/engineer_middleware/include/engineer_middleware/step.h b/engineer_middleware/include/engineer_middleware/step.h index 3f5024d..7239b13 100644 --- a/engineer_middleware/include/engineer_middleware/step.h +++ b/engineer_middleware/include/engineer_middleware/step.h @@ -51,7 +51,9 @@ 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& extend_arm_f_pub, ros::Publisher& extend_arm_b_pub) + ros::Publisher& gimbal_lift_pub, ros::Publisher& extend_arm_f_pub, ros::Publisher& extend_arm_b_pub, + ros::Publisher& silver_lifter_pub, ros::Publisher& silver_pusher_pub, ros::Publisher& silver_rotator_pub, + ros::Publisher& gold_pusher_pub, ros::Publisher& gold_lifter_pub, ros::Publisher& middle_pitch_pub) : planning_result_pub_(planning_result_pub), point_cloud_pub_(point_cloud_pub), arm_group_(arm_group) { ROS_ASSERT(step.hasMember("step")); @@ -100,6 +102,18 @@ class Step } if (step.hasMember("chassis_target")) chassis_target_motion_ = new ChassisTargetMotion(step["chassis_target"], chassis_interface, tf); + if (step.hasMember("silver_lifter")) + silver_lifter_motion_ = new JointPointMotion(step["silver_lifter"], silver_lifter_pub); + if (step.hasMember("silver_pusher")) + silver_pusher_motion_ = new JointPointMotion(step["silver_pusher"], silver_pusher_pub); + if (step.hasMember("silver_rotator")) + silver_rotator_motion_ = new JointPointMotion(step["silver_rotator"], silver_rotator_pub); + if (step.hasMember("gold_pusher")) + gold_pusher_motion_ = new JointPointMotion(step["gold_pusher"], gold_pusher_pub); + if (step.hasMember("gold_lifter")) + gold_lifter_motion_ = new JointPointMotion(step["gold_lifter"], gold_lifter_pub); + if (step.hasMember("middle_pitch")) + middle_pitch_motion_ = new JointPointMotion(step["middle_pitch"], middle_pitch_pub); } bool move() { @@ -143,6 +157,18 @@ class Step success &= extend_arm_front_motion_->move(); if (chassis_target_motion_) success &= chassis_target_motion_->move(); + if (silver_lifter_motion_) + success &= silver_lifter_motion_->move(); + if (silver_pusher_motion_) + success &= silver_pusher_motion_->move(); + if (silver_rotator_motion_) + success &= silver_rotator_motion_->move(); + if (gold_pusher_motion_) + success &= gold_pusher_motion_->move(); + if (gold_lifter_motion_) + success &= gold_lifter_motion_->move(); + if (middle_pitch_motion_) + success &= middle_pitch_motion_->move(); return success; } void stop() @@ -184,6 +210,18 @@ class Step success &= reversal_motion_->isFinish(); if (chassis_target_motion_) success &= chassis_target_motion_->isFinish(); + if (silver_lifter_motion_) + success &= silver_lifter_motion_->isFinish(); + if (silver_pusher_motion_) + success &= silver_pusher_motion_->isFinish(); + if (silver_rotator_motion_) + success &= silver_rotator_motion_->isFinish(); + if (gold_pusher_motion_) + success &= gold_pusher_motion_->isFinish(); + if (gold_lifter_motion_) + success &= gold_lifter_motion_->isFinish(); + if (middle_pitch_motion_) + success &= middle_pitch_motion_->isFinish(); return success; } bool checkTimeout(ros::Duration period) @@ -217,6 +255,8 @@ class Step HandMotion* hand_motion_{}; JointPositionMotion* end_effector_motion_{}; JointPointMotion *ore_rotate_motion_{}, *ore_lift_motion_{}, *gimbal_lift_motion_{}; + JointPointMotion *silver_lifter_motion_{}, *silver_pusher_motion_{}, *silver_rotator_motion_{}, + *gold_pusher_motion_{}, *gold_lifter_motion_{}, *middle_pitch_motion_{}; ExtendMotion *extend_arm_front_motion_{}, *extend_arm_back_motion_{}; StoneNumMotion* stone_num_motion_{}; ChassisMotion* chassis_motion_{}; diff --git a/engineer_middleware/include/engineer_middleware/step_queue.h b/engineer_middleware/include/engineer_middleware/step_queue.h index d2fa912..32424d7 100644 --- a/engineer_middleware/include/engineer_middleware/step_queue.h +++ b/engineer_middleware/include/engineer_middleware/step_queue.h @@ -57,14 +57,17 @@ class StepQueue 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& extend_arm_f_pub, - ros::Publisher& extend_arm_b_pub) + ros::Publisher& extend_arm_b_pub, ros::Publisher& silver_lifter_pub, ros::Publisher& silver_pusher_pub, + ros::Publisher& silver_rotator_pub, ros::Publisher& gold_pusher_pub, ros::Publisher& gold_lifter_pub, + ros::Publisher& middle_pitch_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, extend_arm_f_pub, extend_arm_b_pub); + ore_lift_pub, gimbal_lift_pub, extend_arm_f_pub, extend_arm_b_pub, silver_lifter_pub, + silver_pusher_pub, silver_rotator_pub, gold_pusher_pub, gold_lifter_pub, middle_pitch_pub); } bool run(actionlib::SimpleActionServer& as) { diff --git a/engineer_middleware/src/middleware.cpp b/engineer_middleware/src/middleware.cpp index 686e9f2..34b0105 100644 --- a/engineer_middleware/src/middleware.cpp +++ b/engineer_middleware/src/middleware.cpp @@ -58,6 +58,12 @@ Middleware::Middleware(ros::NodeHandle& nh) , 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)) + , silver_lifter_pub_(nh.advertise("/controllers/silver_lifter_controller/command", 10)) + , silver_pusher_pub_(nh.advertise("/controllers/silver_pusher_controller/command", 10)) + , silver_rotator_pub_(nh.advertise("/controllers/silver_rotator_controller/command", 10)) + , gold_pusher_pub_(nh.advertise("/controllers/gold_pusher_controller/command", 10)) + , gold_lifter_pub_(nh.advertise("/controllers/gold_lifter_controller/command", 10)) + , middle_pitch_pub_(nh.advertise("/controllers/middle_pitch_controller/command", 10)) , tf_listener_(tf_) , is_middleware_control_(false) { @@ -71,11 +77,12 @@ Middleware::Middleware(ros::NodeHandle& nh) ROS_ASSERT(scenes_list.getType() == XmlRpc::XmlRpcValue::Type::TypeStruct); for (XmlRpc::XmlRpcValue::ValueStruct::const_iterator it = steps_list.begin(); it != steps_list.end(); ++it) { - step_queues_.insert( - 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_, extend_arm_f_pub_, extend_arm_b_pub_))); + step_queues_.insert(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_, + extend_arm_f_pub_, extend_arm_b_pub_, silver_lifter_pub_, silver_pusher_pub_, + silver_rotator_pub_, gold_pusher_pub_, gold_lifter_pub_, middle_pitch_pub_))); } } else From 01da3baf2c40320da8d7239725833cb9bb9eb1c9 Mon Sep 17 00:00:00 2001 From: cc0h <7921166012@qq.com> Date: Wed, 10 Jul 2024 17:53:00 +0800 Subject: [PATCH 43/80] Add basic step lists for engineer2. --- .../config/engineer2_steps_list.yaml | 582 +++++++++++++++++- 1 file changed, 580 insertions(+), 2 deletions(-) diff --git a/engineer_middleware/config/engineer2_steps_list.yaml b/engineer_middleware/config/engineer2_steps_list.yaml index debfca0..7bb7e4a 100644 --- a/engineer_middleware/config/engineer2_steps_list.yaml +++ b/engineer_middleware/config/engineer2_steps_list.yaml @@ -7,11 +7,11 @@ common: normally: &NORMALLY speed: 0.4 accel: 0.3 - timeout: 3 + timeout: 5 quickly: &QUICKLY speed: 0.6 accel: 0.6 - timeout: 7. + timeout: 3. tolerance: small_tolerance: &SMALL_TOLERANCE @@ -22,3 +22,581 @@ common: tolerance_joints: [ 0.015, 0.015, 0.02, 0.35, 0.2, 0.2, 0.1 ] max_tolerance: &MAX_TOLERANCE tolerance_joints: [ 0.03, 0.03, 0.03, 0.3, 0.3, 0.3 , 0.3] + + joint1: + home: &JOINT1_HOME_POS + 0.05 + down: &JOINT1_DOWN_POS + 0. + up: &JOINT1_UP_POS + 0.579 + exchange: &JOINT1_EXCHANGE_POS + 0.35 + ready_get_small: &JOINT1_READY_GET_SMALL_POS + 0.4 + get_small: &JOINT1_GET_SMALL_POS + 0.3 + ready_get_stored: &JOINT1_READY_GET_STORED_POS + 0.57 + get_stored: &JOINT1_GET_STORED_POS + 0.5 + get_stored_gold: &JOINT1_GET_STORED_GOLD_POS + 0.029 + + + joint2: + home: &JOINT2_HOME_POS + 0.00001 + exchange: &JOINT2_EXCHANGE_POS + -1.57 + get_small_island: &JOINT2_GET_SMALL_ISLAND_POS + 0.068 + get_stored_silver3: &JOINT2_GET_STORED_SILVER3_POS + -2.12 + get_stored_silver2: &JOINT2_GET_STORED_SILVER2_POS + -2.967 + get_stored_silver1: &JOINT2_GET_STORED_SILVER1_POS + -4.1 + ready_get_stored_gold: &JOINT2_READY_GET_STORED_GOLD_POS + -1.2 + get_stored_gold: &JOINT2_GET_STORED_GOLD_POS + -1.32 + + joint3: + home: &JOINT3_HOME_POS + 0.00001 + get_small_island: &JOINT3_GET_SMALL_ISLAND_POS + -2.18 + get_stored_silver3: &JOINT3_GET_STORED_SILVER3_POS + -1.788 + get_stored_silver2: &JOINT3_GET_STORED_SILVER2_POS + -1.483 + get_stored_silver1: &JOINT3_GET_STORED_SILVER1_POS + 0.00001 + ready_get_stored_gold: &JOINT3_READY_GET_STORED_GOLD_POS + -1.962 + get_stored_gold: &JOINT3_GET_STORED_GOLD_POS + -2.181 + + joint4: + home: &JOINT4_HOME_POS + 0.00001 + r90: &JOINT4_R90_POS + -1.57 + l90: &JOINT4_L90_POS + 1.57 + + + joint5: + home: &JOINT5_HOME_POS + 0.00001 + down_90: &JOINT5_DOWN_90 + 1.57 + get_stored_gold: &JOINT5_GET_STORED_GOLD_POS + 1.193 + + joint6: + home: &JOINT6_HOME_POS + 0.0 + + silver_lift: + home: &SILVER_LIFTER_HOME_POS + 0.0 + lift: &SILVER_LIFTER_LIFT_POS + 0.0 + + silver_push: + home: &SILVER_PUSHER_HOME_POS + 0.0 + get_silver: &SILVER_PUSHER_GET_SILVER_POS + 0.0 + pull_back_little: &SILVER_PUSHER_PULL_BACK_LITTLE_POS + 0.0 + + silver_rotate: + home: &SILVER_ROTATOR_HOME_POS + 0.0 + 90: &SILVER_ROTATOR_90_POS + 1.57 + + gold_push: + home: &GOLD_PUSHER_HOME_POS + 0.0 + get_gold: &GOLD_PUSHER_GET_GOLD_POS + 0.0 + gold_lift: + home: &GOLD_LIFTER_HOME_POS + 0.0 + lift_gold: &GOLD_LIFTER_LIFT_GOLD_POS + 0.0 + + arm: + home: &HOME + joints: [ *JOINT1_HOME_POS, *JOINT2_HOME_POS, *JOINT3_HOME_POS, *JOINT4_HOME_POS, *JOINT5_HOME_POS, *JOINT6_HOME_POS ] + common: + <<: *NORMALLY + tolerance: + <<: *NORMAL_TOLERANCE + + lv4_exchange: &LV4_EXCHANGE + joints: [ *JOINT1_EXCHANGE_POS, *JOINT2_EXCHANGE_POS, *JOINT3_HOME_POS, *JOINT4_HOME_POS, *JOINT5_HOME_POS, *JOINT6_HOME_POS ] + common: + <<: *NORMALLY + tolerance: + <<: *NORMAL_TOLERANCE + + lv5_l_exchange: &LV5_L_EXCHANGE + joints: [ *JOINT1_EXCHANGE_POS, *JOINT2_EXCHANGE_POS, *JOINT3_HOME_POS, *JOINT4_L90_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS] + common: + <<: *NORMALLY + tolerance: + <<: *NORMAL_TOLERANCE + + lv5_r_exchange: &LV5_R_EXCHANGE + joints: [ *JOINT1_EXCHANGE_POS, *JOINT2_EXCHANGE_POS, *JOINT3_HOME_POS, *JOINT4_R90_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS] + common: + <<: *NORMALLY + tolerance: + <<: *NORMAL_TOLERANCE + + + +steps_list: + HOME: + - step: "arm home pos" + arm: + joints: [*JOINT1_HOME_POS, *JOINT2_HOME_POS, *JOINT3_HOME_POS, *JOINT4_HOME_POS, *JOINT5_HOME_POS, *JOINT6_HOME_POS] + common: + <<: *NORMALLY + tolerance: + <<: *NORMAL_TOLERANCE + + LV4_EXCHANGE: + - step: "arm enter exchange pos" + arm: + <<: *LV4_EXCHANGE +# - step: "gimbal ready" + + LV5_L_EXCHANGE: + - step: "arm enter exchange pos" + arm: + <<: *LV5_L_EXCHANGE + + LV5_R_EXCHANGE: + - step: "arm enter exchange pos" + arm: + <<: *LV5_R_EXCHANGE + + + + + GET_SMALL_ISLAND: &GET_SMALL_ISLAND + - step: "ready get small island silver" + arm: + joints: [*JOINT1_READY_GET_SMALL_POS, *JOINT2_GET_SMALL_ISLAND_POS, *JOINT3_GET_SMALL_ISLAND_POS, *JOINT4_HOME_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS] + common: + <<: *NORMALLY + tolerance: + <<: *NORMAL_TOLERANCE + +# - step: "open main gripper" + + - step: "get small island silver" + arm: + joints: [ *JOINT1_GET_SMALL_POS, *JOINT2_GET_SMALL_ISLAND_POS, *JOINT3_GET_SMALL_ISLAND_POS, *JOINT4_HOME_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ] + common: + <<: *NORMALLY + tolerance: + <<: *NORMAL_TOLERANCE + - step: "pull up silver" + arm: + joints: [ *JOINT1_READY_GET_SMALL_POS, *JOINT2_GET_SMALL_ISLAND_POS, *JOINT3_GET_SMALL_ISLAND_POS, *JOINT4_HOME_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ] + common: + <<: *NORMALLY + tolerance: + <<: *NORMAL_TOLERANCE + + STORE_SILVER_TO_GRIPPER1: &STORE_SILVER_TO_GRIPPER1 + - step: "ready to store" + arm: + joints: [*JOINT1_READY_GET_STORED_POS, *JOINT2_GET_STORED_SILVER1_POS, *JOINT3_GET_STORED_SILVER1_POS, *JOINT4_HOME_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS] + common: + <<: *NORMALLY + tolerance: + <<: *NORMAL_TOLERANCE + +# - step: "open gripper1" + - step: "store" + arm: + joints: [ *JOINT1_GET_STORED_POS, *JOINT2_GET_STORED_SILVER1_POS, *JOINT3_GET_STORED_SILVER1_POS, *JOINT4_HOME_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ] + common: + <<: *NORMALLY + tolerance: + <<: *NORMAL_TOLERANCE + # - step: "close main gripper" + + - step: "left arm" + arm: + joints: [ *JOINT1_READY_GET_STORED_POS, *JOINT2_GET_STORED_GOLD_POS, *JOINT3_GET_STORED_SILVER1_POS, *JOINT4_HOME_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ] + common: + <<: *NORMALLY + tolerance: + <<: *NORMAL_TOLERANCE + + STORE_SILVER_TO_GRIPPER2: &STORE_SILVER_TO_GRIPPER2 + - step: "ready to store" + arm: + joints: [ *JOINT1_READY_GET_STORED_POS, *JOINT2_GET_STORED_SILVER2_POS, *JOINT3_GET_STORED_SILVER2_POS, *JOINT4_HOME_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ] + common: + <<: *NORMALLY + tolerance: + <<: *NORMAL_TOLERANCE + + # - step: "open gripper2" + - step: "store" + arm: + joints: [ *JOINT1_GET_STORED_POS, *JOINT2_GET_STORED_SILVER2_POS, *JOINT3_GET_STORED_SILVER2_POS, *JOINT4_HOME_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ] + common: + <<: *NORMALLY + tolerance: + <<: *NORMAL_TOLERANCE + + # - step: "close main gripper" + + - step: "left arm" + arm: + joints: [ *JOINT1_READY_GET_STORED_POS, *JOINT2_GET_STORED_GOLD_POS, *JOINT3_GET_STORED_SILVER2_POS, *JOINT4_HOME_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ] + common: + <<: *NORMALLY + tolerance: + <<: *NORMAL_TOLERANCE + + STORE_SILVER_TO_GRIPPER3: &STORE_SILVER_TO_GRIPPER3 + - step: "ready to store" + arm: + joints: [ *JOINT1_READY_GET_STORED_POS, *JOINT2_GET_STORED_SILVER3_POS, *JOINT3_GET_STORED_SILVER3_POS, *JOINT4_HOME_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ] + common: + <<: *NORMALLY + tolerance: + <<: *NORMAL_TOLERANCE + + # - step: "open gripper3" + - step: "store" + arm: + joints: [ *JOINT1_GET_STORED_POS, *JOINT2_GET_STORED_SILVER3_POS, *JOINT3_GET_STORED_SILVER3_POS, *JOINT4_HOME_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ] + common: + <<: *NORMALLY + tolerance: + <<: *NORMAL_TOLERANCE + + # - step: "close main gripper" + - step: "left arm" + arm: + joints: [ *JOINT1_READY_GET_STORED_POS, *JOINT2_GET_STORED_GOLD_POS, *JOINT3_GET_STORED_SILVER3_POS, *JOINT4_HOME_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ] + common: + <<: *NORMALLY + tolerance: + <<: *NORMAL_TOLERANCE + + GET_SILVER_FROM_GRIPPER1: + - step: "ready to get silver from gripper1" + arm: + joints: [*JOINT1_READY_GET_STORED_POS, *JOINT2_GET_STORED_SILVER1_POS, *JOINT3_GET_STORED_SILVER1_POS, *JOINT4_HOME_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS] + common: + <<: *NORMALLY + tolerance: + <<: *NORMAL_TOLERANCE + + # - step: "open main gripper" + - step: "get silver" + arm: + joints: [ *JOINT1_GET_STORED_POS, *JOINT2_GET_STORED_SILVER1_POS, *JOINT3_GET_STORED_SILVER1_POS, *JOINT4_HOME_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ] + common: + <<: *NORMALLY + tolerance: + <<: *NORMAL_TOLERANCE + # - step: "close gripper1" + + - step: "lift arm" + arm: + joints: [ *JOINT1_READY_GET_STORED_POS, *JOINT2_GET_STORED_GOLD_POS, *JOINT3_GET_STORED_SILVER1_POS, *JOINT4_HOME_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ] + common: + <<: *NORMALLY + tolerance: + <<: *NORMAL_TOLERANCE + + GET_SILVER_FROM_GRIPPER2: + - step: "ready to get silver from gripper2" + arm: + joints: [ *JOINT1_READY_GET_STORED_POS, *JOINT2_GET_STORED_SILVER2_POS, *JOINT3_GET_STORED_SILVER2_POS, *JOINT4_HOME_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ] + common: + <<: *NORMALLY + tolerance: + <<: *NORMAL_TOLERANCE + + # - step: "open main gripper" + - step: "store" + arm: + joints: [ *JOINT1_GET_STORED_POS, *JOINT2_GET_STORED_SILVER2_POS, *JOINT3_GET_STORED_SILVER2_POS, *JOINT4_HOME_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ] + common: + <<: *NORMALLY + tolerance: + <<: *NORMAL_TOLERANCE + + # - step: "close gripper2" + + - step: "lift arm" + arm: + joints: [ *JOINT1_READY_GET_STORED_POS, *JOINT2_GET_STORED_GOLD_POS, *JOINT3_GET_STORED_SILVER2_POS, *JOINT4_HOME_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ] + common: + <<: *NORMALLY + tolerance: + <<: *NORMAL_TOLERANCE + + GET_SILVER_FROM_GRIPPER3: + - step: "ready to get silver from gripper3" + arm: + joints: [ *JOINT1_READY_GET_STORED_POS, *JOINT2_GET_STORED_SILVER3_POS, *JOINT3_GET_STORED_SILVER3_POS, *JOINT4_HOME_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ] + common: + <<: *NORMALLY + tolerance: + <<: *NORMAL_TOLERANCE + + # - step: "open main gripper" + - step: "store" + arm: + joints: [ *JOINT1_GET_STORED_POS, *JOINT2_GET_STORED_SILVER3_POS, *JOINT3_GET_STORED_SILVER3_POS, *JOINT4_HOME_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ] + common: + <<: *NORMALLY + tolerance: + <<: *NORMAL_TOLERANCE + + # - step: "close gripper3" + - step: "left arm" + arm: + joints: [ *JOINT1_READY_GET_STORED_POS, *JOINT2_GET_STORED_GOLD_POS, *JOINT3_GET_STORED_SILVER3_POS, *JOINT4_HOME_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ] + common: + <<: *NORMALLY + tolerance: + <<: *NORMAL_TOLERANCE + + GET_STORED_GOLD: + - step: "ready to get stored gold" + arm: + joints: [ *JOINT1_GET_STORED_GOLD_POS, *JOINT2_GET_STORED_GOLD_POS, *JOINT3_READY_GET_STORED_GOLD_POS, *JOINT4_R90_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ] + common: + <<: *NORMALLY + tolerance: + <<: *NORMAL_TOLERANCE +# - step: "open main gripper" + - step: "get stored gold" + arm: + joints: [ *JOINT1_GET_STORED_GOLD_POS, *JOINT2_GET_STORED_GOLD_POS, *JOINT3_GET_STORED_GOLD_POS, *JOINT4_R90_POS, *JOINT5_GET_STORED_GOLD_POS, *JOINT6_HOME_POS] + common: + <<: *NORMALLY + tolerance: + <<: *NORMAL_TOLERANCE +# - step: "close gold gripper + - step: "move out gold" + arm: + joints: [ *JOINT1_GET_STORED_GOLD_POS, *JOINT2_GET_STORED_GOLD_POS, *JOINT3_READY_GET_STORED_GOLD_POS, *JOINT4_R90_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ] + common: + <<: *NORMALLY + tolerance: + <<: *NORMAL_TOLERANCE + + - step: "lift up gold" + arm: + joints: [ *JOINT1_EXCHANGE_POS, *JOINT2_GET_STORED_GOLD_POS, *JOINT3_READY_GET_STORED_GOLD_POS, *JOINT4_R90_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ] + common: + <<: *NORMALLY + tolerance: + <<: *NORMAL_TOLERANCE + + THREE_SILVER_ARM: + - step: "ready get small island silver" + arm: + joints: [ *JOINT1_READY_GET_SMALL_POS, *JOINT2_GET_SMALL_ISLAND_POS, *JOINT3_GET_SMALL_ISLAND_POS, *JOINT4_HOME_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ] + common: + <<: *NORMALLY + tolerance: + <<: *NORMAL_TOLERANCE + + # - step: "open main gripper" + + - step: "get small island silver" + arm: + joints: [ *JOINT1_GET_SMALL_POS, *JOINT2_GET_SMALL_ISLAND_POS, *JOINT3_GET_SMALL_ISLAND_POS, *JOINT4_HOME_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ] + common: + <<: *NORMALLY + tolerance: + <<: *NORMAL_TOLERANCE + - step: "pull up silver" + arm: + joints: [ *JOINT1_READY_GET_SMALL_POS, *JOINT2_GET_SMALL_ISLAND_POS, *JOINT3_GET_SMALL_ISLAND_POS, *JOINT4_HOME_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ] + common: + <<: *NORMALLY + tolerance: + <<: *NORMAL_TOLERANCE + + - step: "ready to store" + arm: + joints: [ *JOINT1_READY_GET_STORED_POS, *JOINT2_GET_STORED_SILVER2_POS, *JOINT3_GET_STORED_SILVER2_POS, *JOINT4_HOME_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ] + common: + <<: *NORMALLY + tolerance: + <<: *NORMAL_TOLERANCE + + # - step: "open gripper2" + - step: "store" + arm: + joints: [ *JOINT1_GET_STORED_POS, *JOINT2_GET_STORED_SILVER2_POS, *JOINT3_GET_STORED_SILVER2_POS, *JOINT4_HOME_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ] + common: + <<: *NORMALLY + tolerance: + <<: *NORMAL_TOLERANCE + # - step: "close main gripper" + + - step: "left arm" + arm: + joints: [ *JOINT1_READY_GET_STORED_POS, *JOINT2_GET_STORED_SILVER2_POS, *JOINT3_GET_STORED_SILVER2_POS, *JOINT4_HOME_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ] + common: + <<: *NORMALLY + tolerance: + <<: *NORMAL_TOLERANCE + ###################### 2nd silver + - step: "ready get small island silver" + arm: + joints: [ *JOINT1_READY_GET_SMALL_POS, *JOINT2_GET_SMALL_ISLAND_POS, *JOINT3_GET_SMALL_ISLAND_POS, *JOINT4_HOME_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ] + common: + <<: *NORMALLY + tolerance: + <<: *NORMAL_TOLERANCE + + # - step: "open main gripper" + + - step: "get small island silver" + arm: + joints: [ *JOINT1_GET_SMALL_POS, *JOINT2_GET_SMALL_ISLAND_POS, *JOINT3_GET_SMALL_ISLAND_POS, *JOINT4_HOME_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ] + common: + <<: *NORMALLY + tolerance: + <<: *NORMAL_TOLERANCE + - step: "pull up silver" + arm: + joints: [ *JOINT1_READY_GET_SMALL_POS, *JOINT2_GET_SMALL_ISLAND_POS, *JOINT3_GET_SMALL_ISLAND_POS, *JOINT4_HOME_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ] + common: + <<: *NORMALLY + tolerance: + <<: *NORMAL_TOLERANCE + + - step: "ready to store" + arm: + joints: [ *JOINT1_READY_GET_STORED_POS, *JOINT2_GET_STORED_SILVER3_POS, *JOINT3_GET_STORED_SILVER3_POS, *JOINT4_HOME_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ] + common: + <<: *NORMALLY + tolerance: + <<: *NORMAL_TOLERANCE + + # - step: "open gripper3" + - step: "store" + arm: + joints: [ *JOINT1_GET_STORED_POS, *JOINT2_GET_STORED_SILVER3_POS, *JOINT3_GET_STORED_SILVER3_POS, *JOINT4_HOME_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ] + common: + <<: *NORMALLY + tolerance: + <<: *NORMAL_TOLERANCE + # - step: "close main gripper" + + - step: "left arm" + arm: + joints: [ *JOINT1_READY_GET_STORED_POS, *JOINT2_GET_STORED_SILVER3_POS, *JOINT3_GET_STORED_SILVER3_POS, *JOINT4_HOME_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ] + common: + <<: *NORMALLY + tolerance: + <<: *NORMAL_TOLERANCE + ############ 3rd silver + - step: "ready get small island silver" + arm: + joints: [ *JOINT1_READY_GET_SMALL_POS, *JOINT2_GET_SMALL_ISLAND_POS, *JOINT3_GET_SMALL_ISLAND_POS, *JOINT4_HOME_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ] + common: + <<: *NORMALLY + tolerance: + <<: *NORMAL_TOLERANCE + + # - step: "open main gripper" + + - step: "get small island silver" + arm: + joints: [ *JOINT1_GET_SMALL_POS, *JOINT2_GET_SMALL_ISLAND_POS, *JOINT3_GET_SMALL_ISLAND_POS, *JOINT4_HOME_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ] + common: + <<: *NORMALLY + tolerance: + <<: *NORMAL_TOLERANCE + - step: "pull up silver" + arm: + joints: [ *JOINT1_READY_GET_SMALL_POS, *JOINT2_GET_SMALL_ISLAND_POS, *JOINT3_GET_SMALL_ISLAND_POS, *JOINT4_HOME_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ] + common: + <<: *NORMALLY + tolerance: + <<: *NORMAL_TOLERANCE + + THREE_SILVER_GRIPPER: + - step: "rotate down" + silver_rotator: + target: 0.0001 + delay: 0. + - step: "lifter down" + silver_lifter: + target: 0. + delay: 0. + - step: "pusher back" + silver_pusher: + target: 0. + delay: 0.1 +# - step: "open gripper" + - step: "get silver" + silver_pusher: + target: -0.046 + - step: "pusher back little" + silver_pusher: + target: -0.04 + - step: "lifter up" + silver_lifter: + target: 0.18 + delay: 0. + - step: "rotate up" + silver_rotator: + target: 1.57 + - step: "pusher back" + silver_pusher: + target: 0. + delay: 0. + - step: "lifter down" + silver_lifter: + target: 0. + delay: 0. + + GET_MIDDLE_GOLD: + - step: "pusher back" + gold_pusher: + target: 0. + delay: 0. + - step: "lifter down" + gold_lifter: + target: 0 + delay: 0. +# - step: "open gold gripper" + - step: "get gold" + gold_pusher: + target: -0.51 + - step: "lift gold" + gold_lifter: + target: 0.05 + - step: "pull back gold" + gold_pusher: + target: 0 + - step: "lifter down" + gold_lifter: + target: 0 + delay: 0. From f54ac284cc590551694f11cfba674cbbf2afe30a Mon Sep 17 00:00:00 2001 From: cc0h <7921166012@qq.com> Date: Thu, 11 Jul 2024 20:29:32 +0800 Subject: [PATCH 44/80] Record. --- .../config/engineer2_steps_list.yaml | 67 +++++++++++++++---- .../include/engineer_middleware/motion.h | 2 +- 2 files changed, 56 insertions(+), 13 deletions(-) diff --git a/engineer_middleware/config/engineer2_steps_list.yaml b/engineer_middleware/config/engineer2_steps_list.yaml index 7bb7e4a..d8c678c 100644 --- a/engineer_middleware/config/engineer2_steps_list.yaml +++ b/engineer_middleware/config/engineer2_steps_list.yaml @@ -30,6 +30,10 @@ common: 0. up: &JOINT1_UP_POS 0.579 + ready_get_ground: &JOINT1_READY_GET_GROUND_POS + 0.5 + get_ground: &JOINT1_GET_GROUND_POS + 0.4 exchange: &JOINT1_EXCHANGE_POS 0.35 ready_get_small: &JOINT1_READY_GET_SMALL_POS @@ -130,6 +134,12 @@ common: lift_gold: &GOLD_LIFTER_LIFT_GOLD_POS 0.0 + middle_pitch: + ground_stone: &MIDDLE_PITCH_GROUND_STONE + 1.57 + normal: &MIDDLE_PITCH_NORMAL + 0.0 + arm: home: &HOME joints: [ *JOINT1_HOME_POS, *JOINT2_HOME_POS, *JOINT3_HOME_POS, *JOINT4_HOME_POS, *JOINT5_HOME_POS, *JOINT6_HOME_POS ] @@ -190,7 +200,7 @@ steps_list: - GET_SMALL_ISLAND: &GET_SMALL_ISLAND + GET_SMALL_ISLAND: - step: "ready get small island silver" arm: joints: [*JOINT1_READY_GET_SMALL_POS, *JOINT2_GET_SMALL_ISLAND_POS, *JOINT3_GET_SMALL_ISLAND_POS, *JOINT4_HOME_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS] @@ -298,6 +308,38 @@ steps_list: tolerance: <<: *NORMAL_TOLERANCE + GROUND_STONE: + - step: "arm ready get ground stone" + arm: + joints: [*JOINT1_READY_GET_GROUND_POS, *JOINT2_EXCHANGE_POS, *JOINT3_HOME_POS, *JOINT4_HOME_POS, *JOINT5_HOME_POS, *JOINT6_HOME_POS] + common: + <<: *NORMALLY + tolerance: + <<: *NORMAL_TOLERANCE + - step: "ready to get ground stone" + middle_pitch: + target: *MIDDLE_PITCH_GROUND_STONE +# - step: "open main gripper" + - step: "arm down get ground stone" + arm: + joints: [*JOINT1_GET_GROUND_POS, *JOINT2_EXCHANGE_POS, *JOINT3_HOME_POS, *JOINT4_HOME_POS, *JOINT5_HOME_POS, *JOINT6_HOME_POS] + common: + <<: *NORMALLY + tolerance: + <<: *NORMAL_TOLERANCE + - step: "arm pull up stone" + arm: + joints: [*JOINT1_READY_GET_GROUND_POS, *JOINT2_EXCHANGE_POS, *JOINT3_HOME_POS, *JOINT4_HOME_POS, *JOINT5_HOME_POS, *JOINT6_HOME_POS] + common: + <<: *NORMALLY + tolerance: + <<: *NORMAL_TOLERANCE + - step: "middle pitch home" + middle_pitch: + target: *MIDDLE_PITCH_NORMAL + + + GET_SILVER_FROM_GRIPPER1: - step: "ready to get silver from gripper1" arm: @@ -319,7 +361,7 @@ steps_list: - step: "lift arm" arm: - joints: [ *JOINT1_READY_GET_STORED_POS, *JOINT2_GET_STORED_GOLD_POS, *JOINT3_GET_STORED_SILVER1_POS, *JOINT4_HOME_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ] + joints: [ *JOINT1_READY_GET_STORED_POS, *JOINT2_GET_STORED_SILVER1_POS, *JOINT3_GET_STORED_SILVER1_POS, *JOINT4_HOME_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ] common: <<: *NORMALLY tolerance: @@ -347,7 +389,7 @@ steps_list: - step: "lift arm" arm: - joints: [ *JOINT1_READY_GET_STORED_POS, *JOINT2_GET_STORED_GOLD_POS, *JOINT3_GET_STORED_SILVER2_POS, *JOINT4_HOME_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ] + joints: [ *JOINT1_READY_GET_STORED_POS, *JOINT2_GET_STORED_SILVER2_POS, *JOINT3_GET_STORED_SILVER2_POS, *JOINT4_HOME_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ] common: <<: *NORMALLY tolerance: @@ -374,7 +416,7 @@ steps_list: # - step: "close gripper3" - step: "left arm" arm: - joints: [ *JOINT1_READY_GET_STORED_POS, *JOINT2_GET_STORED_GOLD_POS, *JOINT3_GET_STORED_SILVER3_POS, *JOINT4_HOME_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ] + joints: [ *JOINT1_READY_GET_STORED_POS, *JOINT2_GET_STORED_SILVER3_POS, *JOINT3_GET_STORED_SILVER3_POS, *JOINT4_HOME_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ] common: <<: *NORMALLY tolerance: @@ -414,6 +456,9 @@ steps_list: <<: *NORMAL_TOLERANCE THREE_SILVER_ARM: + - step: "silver rotator up" + silver_rotator: + target: 1.57 - step: "ready get small island silver" arm: joints: [ *JOINT1_READY_GET_SMALL_POS, *JOINT2_GET_SMALL_ISLAND_POS, *JOINT3_GET_SMALL_ISLAND_POS, *JOINT4_HOME_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ] @@ -545,11 +590,9 @@ steps_list: - step: "rotate down" silver_rotator: target: 0.0001 - delay: 0. - step: "lifter down" silver_lifter: target: 0. - delay: 0. - step: "pusher back" silver_pusher: target: 0. @@ -558,45 +601,45 @@ steps_list: - step: "get silver" silver_pusher: target: -0.046 + delay: 0.5 - step: "pusher back little" silver_pusher: target: -0.04 + delay: 0.5 - step: "lifter up" silver_lifter: target: 0.18 - delay: 0. - step: "rotate up" silver_rotator: target: 1.57 + delay: 0.5 - step: "pusher back" silver_pusher: target: 0. - delay: 0. - step: "lifter down" silver_lifter: target: 0. - delay: 0. GET_MIDDLE_GOLD: - step: "pusher back" gold_pusher: target: 0. - delay: 0. - step: "lifter down" gold_lifter: target: 0 - delay: 0. # - step: "open gold gripper" - step: "get gold" gold_pusher: target: -0.51 + delay: 0.5 - step: "lift gold" gold_lifter: target: 0.05 + delay: 0.5 - step: "pull back gold" gold_pusher: target: 0 + delay: 0.5 - step: "lifter down" gold_lifter: target: 0 - delay: 0. diff --git a/engineer_middleware/include/engineer_middleware/motion.h b/engineer_middleware/include/engineer_middleware/motion.h index 46024e0..e29665d 100644 --- a/engineer_middleware/include/engineer_middleware/motion.h +++ b/engineer_middleware/include/engineer_middleware/motion.h @@ -675,7 +675,7 @@ class JointPointMotion : public PublishMotion { ROS_ASSERT(motion.hasMember("target")); target_ = xmlRpcGetDouble(motion, "target", 0.0); - delay_ = xmlRpcGetDouble(motion, "delay", 0.5); + delay_ = xmlRpcGetDouble(motion, "delay", 0.0); } bool move() override { From 60c576c621437b5a6081ac804fdbbc4a49249fe1 Mon Sep 17 00:00:00 2001 From: cc0h <7921166012@qq.com> Date: Tue, 16 Jul 2024 10:55:22 +0800 Subject: [PATCH 45/80] Add gimbal controller in engineer2 sim. --- .../launch/load_controllers.launch | 1 + .../engineer2_simulation_controllers.yaml | 29 +++++++++++++++++++ 2 files changed, 30 insertions(+) diff --git a/engineer2_arm_config/launch/load_controllers.launch b/engineer2_arm_config/launch/load_controllers.launch index f7015c2..2b45884 100644 --- a/engineer2_arm_config/launch/load_controllers.launch +++ b/engineer2_arm_config/launch/load_controllers.launch @@ -21,6 +21,7 @@ controllers/robot_state_controller controllers/joint_state_controller controllers/arm_trajectory_controller + controllers/gimbal_controller controllers/chassis_controller controllers/silver_lifter_controller controllers/silver_pusher_controller diff --git a/engineer_middleware/config/engineer2_simulation_controllers.yaml b/engineer_middleware/config/engineer2_simulation_controllers.yaml index e41c049..a43ad6f 100644 --- a/engineer_middleware/config/engineer2_simulation_controllers.yaml +++ b/engineer_middleware/config/engineer2_simulation_controllers.yaml @@ -70,6 +70,35 @@ controllers: joint: right_back_wheel_joint <<: *wheel_setting + gimbal_controller: + type: rm_gimbal_controllers/Controller + yaw: + joint: "yaw_joint" + pid: { p: 0.5, i: 0, d: 0.0, i_clamp_max: 0.0, i_clamp_min: -0.0, antiwindup: true, publish_state: false } + pid_pos: { p: 15.0, i: 0, d: 0.0, i_clamp_max: 0.0, i_clamp_min: 0.0, antiwindup: true, publish_state: true } + k_chassis_vel: 0.035 + k_v: 1.0 + pitch: + joint: "pitch_joint" + pid: { p: 0., i: 0.0, d: 0.0, i_clamp_max: 0.0, i_clamp_min: -0.0, antiwindup: true, publish_state: false } + pid_pos: { p: 0.0, i: 0, d: 0.0, i_clamp_max: 0.0, i_clamp_min: 0.0, antiwindup: true, publish_state: true } + k_v: 1.0 + feedforward: + gravity: 0.0 + enable_gravity_compensation: false + mass_origin: [ 0.0,0.0,0.0 ] + bullet_solver: + resistance_coff_qd_10: 0.45 + resistance_coff_qd_15: 1.0 + resistance_coff_qd_16: 0.7 + resistance_coff_qd_18: 0.55 + resistance_coff_qd_30: 3.0 + g: 9.81 + delay: 0.1 + dt: 0.001 + timeout: 0.001 + publish_rate: 50 + middle_pitch_controller: type: effort_controllers/JointPositionController joint: middle_pitch_joint From 230cf98029fa83a2a54befe54c796173cf17c70e Mon Sep 17 00:00:00 2001 From: cc0h <7921166012@qq.com> Date: Tue, 16 Jul 2024 22:45:12 +0800 Subject: [PATCH 46/80] Modify gpio motion to control multi gpio ports. --- .../config/engineer2_steps_list.yaml | 107 ++++++++++++++++-- .../include/engineer_middleware/motion.h | 29 ++++- 2 files changed, 124 insertions(+), 12 deletions(-) diff --git a/engineer_middleware/config/engineer2_steps_list.yaml b/engineer_middleware/config/engineer2_steps_list.yaml index d8c678c..eeb3f90 100644 --- a/engineer_middleware/config/engineer2_steps_list.yaml +++ b/engineer_middleware/config/engineer2_steps_list.yaml @@ -169,6 +169,43 @@ common: tolerance: <<: *NORMAL_TOLERANCE + gripper: + open_main_gripper: &OPEN_MAIN_GRIPPER + pin: 0 + state: true + close_main_gripper: &CLOSE_MAIN_GRIPPER + pin: 0 + state: false + open_s1: &OPEN_S1 + pin: 1 + state: true + close_s1: &CLOSE_S1 + pin: 1 + state: false + open_s2: &OPEN_S2 + pin: 2 + state: true + close_s2: &CLOSE_S2 + pin: 2 + state: false + open_s3: &OPEN_S3 + pin: 3 + state: true + close_s3: &CLOSE_S3 + pin: 3 + state: false + open_gold: &OPEN_GOLD + pin: 4 + state: true + close_gold: &CLOSE_GOLD + pin: 4 + state: false + open_s_gripper: &OPEN_S_GRIPPER + pin: 5 + state: true + close_s_gripper: &CLOSE_S_GRIPPER + pin: 5 + state: false steps_list: @@ -226,7 +263,7 @@ steps_list: tolerance: <<: *NORMAL_TOLERANCE - STORE_SILVER_TO_GRIPPER1: &STORE_SILVER_TO_GRIPPER1 + STORE_SILVER_TO_GRIPPER1: - step: "ready to store" arm: joints: [*JOINT1_READY_GET_STORED_POS, *JOINT2_GET_STORED_SILVER1_POS, *JOINT3_GET_STORED_SILVER1_POS, *JOINT4_HOME_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS] @@ -253,7 +290,7 @@ steps_list: tolerance: <<: *NORMAL_TOLERANCE - STORE_SILVER_TO_GRIPPER2: &STORE_SILVER_TO_GRIPPER2 + STORE_SILVER_TO_GRIPPER2: - step: "ready to store" arm: joints: [ *JOINT1_READY_GET_STORED_POS, *JOINT2_GET_STORED_SILVER2_POS, *JOINT3_GET_STORED_SILVER2_POS, *JOINT4_HOME_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ] @@ -281,7 +318,7 @@ steps_list: tolerance: <<: *NORMAL_TOLERANCE - STORE_SILVER_TO_GRIPPER3: &STORE_SILVER_TO_GRIPPER3 + STORE_SILVER_TO_GRIPPER3: - step: "ready to store" arm: joints: [ *JOINT1_READY_GET_STORED_POS, *JOINT2_GET_STORED_SILVER3_POS, *JOINT3_GET_STORED_SILVER3_POS, *JOINT4_HOME_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ] @@ -340,7 +377,7 @@ steps_list: - GET_SILVER_FROM_GRIPPER1: + GET_STORED_SILVER1: - step: "ready to get silver from gripper1" arm: joints: [*JOINT1_READY_GET_STORED_POS, *JOINT2_GET_STORED_SILVER1_POS, *JOINT3_GET_STORED_SILVER1_POS, *JOINT4_HOME_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS] @@ -367,7 +404,7 @@ steps_list: tolerance: <<: *NORMAL_TOLERANCE - GET_SILVER_FROM_GRIPPER2: + GET_STORED_SILVER2: - step: "ready to get silver from gripper2" arm: joints: [ *JOINT1_READY_GET_STORED_POS, *JOINT2_GET_STORED_SILVER2_POS, *JOINT3_GET_STORED_SILVER2_POS, *JOINT4_HOME_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ] @@ -395,7 +432,7 @@ steps_list: tolerance: <<: *NORMAL_TOLERANCE - GET_SILVER_FROM_GRIPPER3: + GET_STORED_SILVER3: - step: "ready to get silver from gripper3" arm: joints: [ *JOINT1_READY_GET_STORED_POS, *JOINT2_GET_STORED_SILVER3_POS, *JOINT3_GET_STORED_SILVER3_POS, *JOINT4_HOME_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ] @@ -455,7 +492,7 @@ steps_list: tolerance: <<: *NORMAL_TOLERANCE - THREE_SILVER_ARM: + ARM_THREE_SILVER: - step: "silver rotator up" silver_rotator: target: 1.57 @@ -586,7 +623,7 @@ steps_list: tolerance: <<: *NORMAL_TOLERANCE - THREE_SILVER_GRIPPER: + GRIPPER_THREE_SILVER: - step: "rotate down" silver_rotator: target: 0.0001 @@ -620,7 +657,7 @@ steps_list: silver_lifter: target: 0. - GET_MIDDLE_GOLD: + MID_BIG_ISLAND: - step: "pusher back" gold_pusher: target: 0. @@ -643,3 +680,55 @@ steps_list: - step: "lifter down" gold_lifter: target: 0 + + TEST1: + - step: "main open" + gripper: + <<: *OPEN_MAIN_GRIPPER + +# + TEST2: + - step: "main close" + gripper: + <<: *CLOSE_MAIN_GRIPPER + TEST3: + - step: "s1 open" + gripper: + <<: *OPEN_S1 + TEST4: + - step: "s1 c" + gripper: + <<: *CLOSE_S1 + TEST5: + - step: "s2 open" + gripper: + <<: *OPEN_S2 + + TEST6: + - step: "s2 close" + gripper: + <<: *CLOSE_S2 + TEST7: + - step: "s3 open" + gripper: + <<: *OPEN_S3 + TEST8: + - step: "s3 close" + gripper: + <<: *CLOSE_S3 + TEST9: + - step: "g1 open" + gripper: + <<: *OPEN_GOLD + TEST10: + - step: "g1 close" + gripper: + <<: *CLOSE_GOLD + TEST11: + - step: "g1 close" + gripper: + <<: *OPEN_S_GRIPPER + TEST12: + - step: "g1 close" + gripper: + <<: *CLOSE_S_GRIPPER diff --git a/engineer_middleware/include/engineer_middleware/motion.h b/engineer_middleware/include/engineer_middleware/motion.h index e29665d..ffbd525 100644 --- a/engineer_middleware/include/engineer_middleware/motion.h +++ b/engineer_middleware/include/engineer_middleware/motion.h @@ -528,18 +528,41 @@ class GpioMotion : public PublishMotion GpioMotion(XmlRpc::XmlRpcValue& motion, ros::Publisher& interface) : PublishMotion(motion, interface) { + msg_.gpio_state.assign(6, false); + msg_.gpio_name.assign(6, "no_registered"); + pin_ = motion["pin"]; state_ = motion["state"]; - msg_.gpio_state.push_back(0); - msg_.gpio_name.push_back("gripper"); + switch (pin_) + { + case 0: + msg_.gpio_name[0] = "main_gripper"; + break; + case 1: + msg_.gpio_name[1] = "silver_gripper1"; + break; + case 2: + msg_.gpio_name[2] = "silver_gripper2"; + break; + case 3: + msg_.gpio_name[3] = "silver_gripper3"; + break; + case 4: + msg_.gpio_name[4] = "gold_gripper"; + break; + case 5: + msg_.gpio_name[5] = "silver_pump"; + break; + } } bool move() override { - msg_.gpio_state[0] = state_; + msg_.gpio_state[pin_] = state_; return PublishMotion::move(); } private: bool state_; + int pin_; }; class StoneNumMotion : public PublishMotion From 464d2a442ed20ea99763d6785c5653d64fc98632 Mon Sep 17 00:00:00 2001 From: cc0h <7921166012@qq.com> Date: Wed, 17 Jul 2024 10:29:46 +0800 Subject: [PATCH 47/80] Modify gpio motion to avoid invalid gpio port. --- .../include/engineer_middleware/motion.h | 14 +++++++++++--- 1 file changed, 11 insertions(+), 3 deletions(-) diff --git a/engineer_middleware/include/engineer_middleware/motion.h b/engineer_middleware/include/engineer_middleware/motion.h index ffbd525..0827a84 100644 --- a/engineer_middleware/include/engineer_middleware/motion.h +++ b/engineer_middleware/include/engineer_middleware/motion.h @@ -528,8 +528,8 @@ class GpioMotion : public PublishMotion GpioMotion(XmlRpc::XmlRpcValue& motion, ros::Publisher& interface) : PublishMotion(motion, interface) { - msg_.gpio_state.assign(6, false); - msg_.gpio_name.assign(6, "no_registered"); + msg_.gpio_state.assign(8, false); + msg_.gpio_name.assign(8, "no_registered"); pin_ = motion["pin"]; state_ = motion["state"]; switch (pin_) @@ -550,7 +550,15 @@ class GpioMotion : public PublishMotion msg_.gpio_name[4] = "gold_gripper"; break; case 5: - msg_.gpio_name[5] = "silver_pump"; + msg_.gpio_name[5] = "unused"; + ROS_WARN_STREAM("GPIO port 6 is unused now!"); + break; + case 6: + msg_.gpio_name[6] = "unused"; + ROS_WARN_STREAM("GPIO port 7 is unused now!"); + break; + case 7: + msg_.gpio_name[7] = "silver_pump"; break; } } From 4b35ef9a475c1786325547bd3f35f17c0e40a5e0 Mon Sep 17 00:00:00 2001 From: cc0h <7921166012@qq.com> Date: Thu, 18 Jul 2024 19:06:13 +0800 Subject: [PATCH 48/80] Add stone num motion. --- .../config/engineer2_steps_list.yaml | 115 ++++++++---------- 1 file changed, 54 insertions(+), 61 deletions(-) diff --git a/engineer_middleware/config/engineer2_steps_list.yaml b/engineer_middleware/config/engineer2_steps_list.yaml index eeb3f90..ea40b6d 100644 --- a/engineer_middleware/config/engineer2_steps_list.yaml +++ b/engineer_middleware/config/engineer2_steps_list.yaml @@ -41,7 +41,7 @@ common: get_small: &JOINT1_GET_SMALL_POS 0.3 ready_get_stored: &JOINT1_READY_GET_STORED_POS - 0.57 + 0.545 get_stored: &JOINT1_GET_STORED_POS 0.5 get_stored_gold: &JOINT1_GET_STORED_GOLD_POS @@ -200,15 +200,19 @@ common: close_gold: &CLOSE_GOLD pin: 4 state: false - open_s_gripper: &OPEN_S_GRIPPER - pin: 5 + open_silver_gripper: &OPEN_S_GRIPPER + pin: 7 state: true - close_s_gripper: &CLOSE_S_GRIPPER - pin: 5 + close_silver_gripper: &CLOSE_S_GRIPPER + pin: 7 state: false steps_list: + MIDDLE_PITCH_UP: + - step: "middld pitch up" + middle_pitch: + target: *MIDDLE_PITCH_NORMAL HOME: - step: "arm home pos" arm: @@ -218,6 +222,18 @@ steps_list: tolerance: <<: *NORMAL_TOLERANCE + HOME_WITH_PITCH: + - step: "arm home pos" + arm: + joints: [ *JOINT1_HOME_POS, *JOINT2_HOME_POS, *JOINT3_HOME_POS, *JOINT4_HOME_POS, *JOINT5_HOME_POS, *JOINT6_HOME_POS ] + common: + <<: *NORMALLY + tolerance: + <<: *NORMAL_TOLERANCE + - step: "middle pitch up" + middle_pitch: + target: *MIDDLE_PITCH_NORMAL + LV4_EXCHANGE: - step: "arm enter exchange pos" arm: @@ -281,7 +297,9 @@ steps_list: tolerance: <<: *NORMAL_TOLERANCE # - step: "close main gripper" - + - step: "add stone" + stone_num: + change: "+s1" - step: "left arm" arm: joints: [ *JOINT1_READY_GET_STORED_POS, *JOINT2_GET_STORED_GOLD_POS, *JOINT3_GET_STORED_SILVER1_POS, *JOINT4_HOME_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ] @@ -309,7 +327,9 @@ steps_list: <<: *NORMAL_TOLERANCE # - step: "close main gripper" - + - step: "add stone" + stone_num: + change: "+s2" - step: "left arm" arm: joints: [ *JOINT1_READY_GET_STORED_POS, *JOINT2_GET_STORED_GOLD_POS, *JOINT3_GET_STORED_SILVER2_POS, *JOINT4_HOME_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ] @@ -337,6 +357,9 @@ steps_list: <<: *NORMAL_TOLERANCE # - step: "close main gripper" + - step: "add stone" + stone_num: + change: "+s3" - step: "left arm" arm: joints: [ *JOINT1_READY_GET_STORED_POS, *JOINT2_GET_STORED_GOLD_POS, *JOINT3_GET_STORED_SILVER3_POS, *JOINT4_HOME_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ] @@ -395,7 +418,9 @@ steps_list: tolerance: <<: *NORMAL_TOLERANCE # - step: "close gripper1" - + - step: "minus stone" + stone_num: + change: "-s1" - step: "lift arm" arm: joints: [ *JOINT1_READY_GET_STORED_POS, *JOINT2_GET_STORED_SILVER1_POS, *JOINT3_GET_STORED_SILVER1_POS, *JOINT4_HOME_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ] @@ -423,7 +448,9 @@ steps_list: <<: *NORMAL_TOLERANCE # - step: "close gripper2" - + - step: "minus stone" + stone_num: + change: "-s2" - step: "lift arm" arm: joints: [ *JOINT1_READY_GET_STORED_POS, *JOINT2_GET_STORED_SILVER2_POS, *JOINT3_GET_STORED_SILVER2_POS, *JOINT4_HOME_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ] @@ -451,6 +478,9 @@ steps_list: <<: *NORMAL_TOLERANCE # - step: "close gripper3" + - step: "minus stone" + stone_num: + change: "-s3" - step: "left arm" arm: joints: [ *JOINT1_READY_GET_STORED_POS, *JOINT2_GET_STORED_SILVER3_POS, *JOINT3_GET_STORED_SILVER3_POS, *JOINT4_HOME_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ] @@ -476,6 +506,9 @@ steps_list: tolerance: <<: *NORMAL_TOLERANCE # - step: "close gold gripper + - step: "add stone" + stone_num: + change: "+g" - step: "move out gold" arm: joints: [ *JOINT1_GET_STORED_GOLD_POS, *JOINT2_GET_STORED_GOLD_POS, *JOINT3_READY_GET_STORED_GOLD_POS, *JOINT4_R90_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ] @@ -656,6 +689,15 @@ steps_list: - step: "lifter down" silver_lifter: target: 0. + - step: "add stone1" + stone_num: + change: "+s1" + - step: "add stone1" + stone_num: + change: "+s2" + - step: "add stone1" + stone_num: + change: "+s3" MID_BIG_ISLAND: - step: "pusher back" @@ -680,55 +722,6 @@ steps_list: - step: "lifter down" gold_lifter: target: 0 - - TEST1: - - step: "main open" - gripper: - <<: *OPEN_MAIN_GRIPPER - -# - TEST2: - - step: "main close" - gripper: - <<: *CLOSE_MAIN_GRIPPER - TEST3: - - step: "s1 open" - gripper: - <<: *OPEN_S1 - TEST4: - - step: "s1 c" - gripper: - <<: *CLOSE_S1 - TEST5: - - step: "s2 open" - gripper: - <<: *OPEN_S2 - - TEST6: - - step: "s2 close" - gripper: - <<: *CLOSE_S2 - TEST7: - - step: "s3 open" - gripper: - <<: *OPEN_S3 - TEST8: - - step: "s3 close" - gripper: - <<: *CLOSE_S3 - TEST9: - - step: "g1 open" - gripper: - <<: *OPEN_GOLD - TEST10: - - step: "g1 close" - gripper: - <<: *CLOSE_GOLD - TEST11: - - step: "g1 close" - gripper: - <<: *OPEN_S_GRIPPER - TEST12: - - step: "g1 close" - gripper: - <<: *CLOSE_S_GRIPPER + - step: "add stone" + stone_num: + change: "+g" From f55c27797e4c425c9a44607035e68e49024f4570 Mon Sep 17 00:00:00 2001 From: cc0h <7921166012@qq.com> Date: Fri, 19 Jul 2024 01:35:20 +0800 Subject: [PATCH 49/80] Improve step lists of engineer2. --- .../config/engineer2_steps_list.yaml | 314 ++++++++++++------ 1 file changed, 216 insertions(+), 98 deletions(-) diff --git a/engineer_middleware/config/engineer2_steps_list.yaml b/engineer_middleware/config/engineer2_steps_list.yaml index ea40b6d..de2a86e 100644 --- a/engineer_middleware/config/engineer2_steps_list.yaml +++ b/engineer_middleware/config/engineer2_steps_list.yaml @@ -25,9 +25,9 @@ common: joint1: home: &JOINT1_HOME_POS - 0.05 + 0.1 down: &JOINT1_DOWN_POS - 0. + 0.0000001 up: &JOINT1_UP_POS 0.579 ready_get_ground: &JOINT1_READY_GET_GROUND_POS @@ -50,9 +50,15 @@ common: joint2: home: &JOINT2_HOME_POS - 0.00001 + 1.0 + zero: &JOINT2_ZERO_POS + 0.0000001 exchange: &JOINT2_EXCHANGE_POS -1.57 + l_exchange: &JOINT2_L_EXCHANGE_POS + -0.984 + r_exchange: &JOINT2_R_EXCHANGE_POS + -2.156 get_small_island: &JOINT2_GET_SMALL_ISLAND_POS 0.068 get_stored_silver3: &JOINT2_GET_STORED_SILVER3_POS @@ -60,7 +66,7 @@ common: get_stored_silver2: &JOINT2_GET_STORED_SILVER2_POS -2.967 get_stored_silver1: &JOINT2_GET_STORED_SILVER1_POS - -4.1 + -3.75 ready_get_stored_gold: &JOINT2_READY_GET_STORED_GOLD_POS -1.2 get_stored_gold: &JOINT2_GET_STORED_GOLD_POS @@ -68,7 +74,13 @@ common: joint3: home: &JOINT3_HOME_POS - 0.00001 + 0.9 + zero: &JOINT3_ZERO_POS + 0.00000001 + l_exchange: &JOINT3_L_EXCHANGE_POS + -1.165 + r_exchange: &JOINT3_R_EXCHANGE_POS + 1.165 get_small_island: &JOINT3_GET_SMALL_ISLAND_POS -2.18 get_stored_silver3: &JOINT3_GET_STORED_SILVER3_POS @@ -84,7 +96,9 @@ common: joint4: home: &JOINT4_HOME_POS - 0.00001 + 1.022 + zero: &JOINT4_ZERO_POS + 0.000001 r90: &JOINT4_R90_POS -1.57 l90: &JOINT4_L90_POS @@ -102,6 +116,10 @@ common: joint6: home: &JOINT6_HOME_POS 0.0 + l_90: &JOINT6_L_90 + 1.57 + r_90: &JOINT6_R_90 + -1.57 silver_lift: home: &SILVER_LIFTER_HOME_POS @@ -149,21 +167,21 @@ common: <<: *NORMAL_TOLERANCE lv4_exchange: &LV4_EXCHANGE - joints: [ *JOINT1_EXCHANGE_POS, *JOINT2_EXCHANGE_POS, *JOINT3_HOME_POS, *JOINT4_HOME_POS, *JOINT5_HOME_POS, *JOINT6_HOME_POS ] + joints: [ *JOINT1_EXCHANGE_POS, *JOINT2_EXCHANGE_POS, *JOINT3_HOME_POS, *JOINT4_ZERO_POS, *JOINT5_HOME_POS, *JOINT6_HOME_POS ] common: <<: *NORMALLY tolerance: <<: *NORMAL_TOLERANCE lv5_l_exchange: &LV5_L_EXCHANGE - joints: [ *JOINT1_EXCHANGE_POS, *JOINT2_EXCHANGE_POS, *JOINT3_HOME_POS, *JOINT4_L90_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS] + joints: [ *JOINT1_EXCHANGE_POS, *JOINT2_L_EXCHANGE_POS, *JOINT3_L_EXCHANGE_POS, *JOINT4_R90_POS, *JOINT5_DOWN_90, *JOINT6_L_90] common: <<: *NORMALLY tolerance: <<: *NORMAL_TOLERANCE lv5_r_exchange: &LV5_R_EXCHANGE - joints: [ *JOINT1_EXCHANGE_POS, *JOINT2_EXCHANGE_POS, *JOINT3_HOME_POS, *JOINT4_R90_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS] + joints: [ *JOINT1_EXCHANGE_POS, *JOINT2_R_EXCHANGE_POS, *JOINT3_R_EXCHANGE_POS, *JOINT4_L90_POS, *JOINT5_DOWN_90, *JOINT6_R_90] common: <<: *NORMALLY tolerance: @@ -252,117 +270,165 @@ steps_list: + OPEN_SILVER_GRIPPER: + - step: "open silver gripper" + gripper: + <<: *OPEN_S_GRIPPER + + CLOSE_SILVER_GRIPPER: + - step: "close silver gripper" + gripper: + <<: *CLOSE_S_GRIPPER GET_SMALL_ISLAND: - step: "ready get small island silver" arm: - joints: [*JOINT1_READY_GET_SMALL_POS, *JOINT2_GET_SMALL_ISLAND_POS, *JOINT3_GET_SMALL_ISLAND_POS, *JOINT4_HOME_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS] + joints: [*JOINT1_READY_GET_SMALL_POS, *JOINT2_GET_SMALL_ISLAND_POS, *JOINT3_GET_SMALL_ISLAND_POS, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS] common: <<: *NORMALLY tolerance: <<: *NORMAL_TOLERANCE - -# - step: "open main gripper" - + - step: "open main gripper" + gripper: + <<: *OPEN_MAIN_GRIPPER - step: "get small island silver" arm: - joints: [ *JOINT1_GET_SMALL_POS, *JOINT2_GET_SMALL_ISLAND_POS, *JOINT3_GET_SMALL_ISLAND_POS, *JOINT4_HOME_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ] + joints: [ *JOINT1_GET_SMALL_POS, *JOINT2_GET_SMALL_ISLAND_POS, *JOINT3_GET_SMALL_ISLAND_POS, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ] common: <<: *NORMALLY tolerance: <<: *NORMAL_TOLERANCE - step: "pull up silver" arm: - joints: [ *JOINT1_READY_GET_SMALL_POS, *JOINT2_GET_SMALL_ISLAND_POS, *JOINT3_GET_SMALL_ISLAND_POS, *JOINT4_HOME_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ] + joints: [ *JOINT1_READY_GET_SMALL_POS, *JOINT2_GET_SMALL_ISLAND_POS, *JOINT3_GET_SMALL_ISLAND_POS, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ] common: <<: *NORMALLY tolerance: <<: *NORMAL_TOLERANCE STORE_SILVER_TO_GRIPPER1: + - step: "silver rotator up" + silver_rotator: + target: 1.57 - step: "ready to store" arm: - joints: [*JOINT1_READY_GET_STORED_POS, *JOINT2_GET_STORED_SILVER1_POS, *JOINT3_GET_STORED_SILVER1_POS, *JOINT4_HOME_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS] + joints: [*JOINT1_READY_GET_STORED_POS, *JOINT2_GET_STORED_SILVER1_POS, *JOINT3_GET_STORED_SILVER1_POS, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS] common: <<: *NORMALLY tolerance: <<: *NORMAL_TOLERANCE - -# - step: "open gripper1" + - step: "open gripper1" + gripper: + <<: *OPEN_S1 - step: "store" arm: - joints: [ *JOINT1_GET_STORED_POS, *JOINT2_GET_STORED_SILVER1_POS, *JOINT3_GET_STORED_SILVER1_POS, *JOINT4_HOME_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ] + joints: [ *JOINT1_GET_STORED_POS, *JOINT2_GET_STORED_SILVER1_POS, *JOINT3_GET_STORED_SILVER1_POS, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ] common: <<: *NORMALLY tolerance: <<: *NORMAL_TOLERANCE - # - step: "close main gripper" + - step: "close main gripper" + gripper: + <<: *CLOSE_MAIN_GRIPPER - step: "add stone" stone_num: change: "+s1" + - step: "lift up arm" + arm: + joints: [ *JOINT1_READY_GET_STORED_POS, *JOINT2_GET_STORED_SILVER1_POS, *JOINT3_GET_STORED_SILVER1_POS, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ] + common: + <<: *NORMALLY + tolerance: + <<: *NORMAL_TOLERANCE - step: "left arm" arm: - joints: [ *JOINT1_READY_GET_STORED_POS, *JOINT2_GET_STORED_GOLD_POS, *JOINT3_GET_STORED_SILVER1_POS, *JOINT4_HOME_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ] + joints: [ *JOINT1_READY_GET_STORED_POS, 0.113, -2.561, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ] common: <<: *NORMALLY tolerance: <<: *NORMAL_TOLERANCE STORE_SILVER_TO_GRIPPER2: + - step: "silver rotator up" + silver_rotator: + target: 1.57 - step: "ready to store" arm: - joints: [ *JOINT1_READY_GET_STORED_POS, *JOINT2_GET_STORED_SILVER2_POS, *JOINT3_GET_STORED_SILVER2_POS, *JOINT4_HOME_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ] + joints: [ *JOINT1_READY_GET_STORED_POS, *JOINT2_GET_STORED_SILVER2_POS, *JOINT3_GET_STORED_SILVER2_POS, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ] common: <<: *NORMALLY tolerance: <<: *NORMAL_TOLERANCE - - # - step: "open gripper2" + - step: "open gripper2" + gripper: + <<: *OPEN_S2 - step: "store" arm: - joints: [ *JOINT1_GET_STORED_POS, *JOINT2_GET_STORED_SILVER2_POS, *JOINT3_GET_STORED_SILVER2_POS, *JOINT4_HOME_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ] + joints: [ *JOINT1_GET_STORED_POS, *JOINT2_GET_STORED_SILVER2_POS, *JOINT3_GET_STORED_SILVER2_POS, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ] common: <<: *NORMALLY tolerance: <<: *NORMAL_TOLERANCE - - # - step: "close main gripper" + - step: "close main gripper" + gripper: + <<: *CLOSE_MAIN_GRIPPER - step: "add stone" stone_num: change: "+s2" + - step: "lift up arm" + arm: + joints: [ *JOINT1_READY_GET_STORED_POS, *JOINT2_GET_STORED_SILVER2_POS, *JOINT3_GET_STORED_SILVER2_POS, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ] + common: + <<: *NORMALLY + tolerance: + <<: *NORMAL_TOLERANCE - step: "left arm" arm: - joints: [ *JOINT1_READY_GET_STORED_POS, *JOINT2_GET_STORED_GOLD_POS, *JOINT3_GET_STORED_SILVER2_POS, *JOINT4_HOME_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ] + joints: [ *JOINT1_READY_GET_STORED_POS, 0.113, -2.561, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ] common: <<: *NORMALLY tolerance: <<: *NORMAL_TOLERANCE STORE_SILVER_TO_GRIPPER3: + - step: "silver rotator up" + silver_rotator: + target: 1.57 - step: "ready to store" arm: - joints: [ *JOINT1_READY_GET_STORED_POS, *JOINT2_GET_STORED_SILVER3_POS, *JOINT3_GET_STORED_SILVER3_POS, *JOINT4_HOME_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ] + joints: [ *JOINT1_READY_GET_STORED_POS, *JOINT2_GET_STORED_SILVER3_POS, *JOINT3_GET_STORED_SILVER3_POS, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ] common: <<: *NORMALLY tolerance: <<: *NORMAL_TOLERANCE - # - step: "open gripper3" + - step: "open gripper3" + gripper: + <<: *OPEN_S3 - step: "store" arm: - joints: [ *JOINT1_GET_STORED_POS, *JOINT2_GET_STORED_SILVER3_POS, *JOINT3_GET_STORED_SILVER3_POS, *JOINT4_HOME_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ] + joints: [ *JOINT1_GET_STORED_POS, *JOINT2_GET_STORED_SILVER3_POS, *JOINT3_GET_STORED_SILVER3_POS, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ] common: <<: *NORMALLY tolerance: <<: *NORMAL_TOLERANCE - # - step: "close main gripper" + - step: "close main gripper" + gripper: + <<: *CLOSE_MAIN_GRIPPER - step: "add stone" stone_num: change: "+s3" + - step: "lift up arm" + arm: + joints: [ *JOINT1_READY_GET_STORED_POS, *JOINT2_GET_STORED_SILVER3_POS, *JOINT3_GET_STORED_SILVER3_POS, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ] + common: + <<: *NORMALLY + tolerance: + <<: *NORMAL_TOLERANCE - step: "left arm" arm: - joints: [ *JOINT1_READY_GET_STORED_POS, *JOINT2_GET_STORED_GOLD_POS, *JOINT3_GET_STORED_SILVER3_POS, *JOINT4_HOME_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ] + joints: [ *JOINT1_READY_GET_STORED_POS, 0.113, -2.561, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ] common: <<: *NORMALLY tolerance: @@ -371,7 +437,7 @@ steps_list: GROUND_STONE: - step: "arm ready get ground stone" arm: - joints: [*JOINT1_READY_GET_GROUND_POS, *JOINT2_EXCHANGE_POS, *JOINT3_HOME_POS, *JOINT4_HOME_POS, *JOINT5_HOME_POS, *JOINT6_HOME_POS] + joints: [*JOINT1_READY_GET_GROUND_POS, *JOINT2_EXCHANGE_POS, *JOINT3_ZERO_POS, *JOINT4_ZERO_POS, *JOINT5_HOME_POS, *JOINT6_HOME_POS] common: <<: *NORMALLY tolerance: @@ -379,17 +445,19 @@ steps_list: - step: "ready to get ground stone" middle_pitch: target: *MIDDLE_PITCH_GROUND_STONE -# - step: "open main gripper" + - step: "open main gripper" + gripper: + <<: *OPEN_MAIN_GRIPPER - step: "arm down get ground stone" arm: - joints: [*JOINT1_GET_GROUND_POS, *JOINT2_EXCHANGE_POS, *JOINT3_HOME_POS, *JOINT4_HOME_POS, *JOINT5_HOME_POS, *JOINT6_HOME_POS] + joints: [*JOINT1_GET_GROUND_POS, *JOINT2_EXCHANGE_POS, *JOINT3_ZERO_POS, *JOINT4_ZERO_POS, *JOINT5_HOME_POS, *JOINT6_HOME_POS] common: <<: *NORMALLY tolerance: <<: *NORMAL_TOLERANCE - step: "arm pull up stone" arm: - joints: [*JOINT1_READY_GET_GROUND_POS, *JOINT2_EXCHANGE_POS, *JOINT3_HOME_POS, *JOINT4_HOME_POS, *JOINT5_HOME_POS, *JOINT6_HOME_POS] + joints: [*JOINT1_READY_GET_GROUND_POS, *JOINT2_EXCHANGE_POS, *JOINT3_ZERO_POS, *JOINT4_ZERO_POS, *JOINT5_HOME_POS, *JOINT6_HOME_POS] common: <<: *NORMALLY tolerance: @@ -403,27 +471,38 @@ steps_list: GET_STORED_SILVER1: - step: "ready to get silver from gripper1" arm: - joints: [*JOINT1_READY_GET_STORED_POS, *JOINT2_GET_STORED_SILVER1_POS, *JOINT3_GET_STORED_SILVER1_POS, *JOINT4_HOME_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS] + joints: [*JOINT1_READY_GET_STORED_POS, *JOINT2_GET_STORED_SILVER1_POS, *JOINT3_GET_STORED_SILVER1_POS, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS] common: <<: *NORMALLY tolerance: <<: *NORMAL_TOLERANCE - # - step: "open main gripper" + - step: "open main gripper" + gripper: + <<: *OPEN_MAIN_GRIPPER - step: "get silver" arm: - joints: [ *JOINT1_GET_STORED_POS, *JOINT2_GET_STORED_SILVER1_POS, *JOINT3_GET_STORED_SILVER1_POS, *JOINT4_HOME_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ] + joints: [ *JOINT1_GET_STORED_POS, *JOINT2_GET_STORED_SILVER1_POS, *JOINT3_GET_STORED_SILVER1_POS, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ] common: <<: *NORMALLY tolerance: <<: *NORMAL_TOLERANCE - # - step: "close gripper1" + - step: "close gripper1" + gripper: + <<: *CLOSE_S1 - step: "minus stone" stone_num: change: "-s1" - step: "lift arm" arm: - joints: [ *JOINT1_READY_GET_STORED_POS, *JOINT2_GET_STORED_SILVER1_POS, *JOINT3_GET_STORED_SILVER1_POS, *JOINT4_HOME_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ] + joints: [ *JOINT1_READY_GET_STORED_POS, *JOINT2_GET_STORED_SILVER1_POS, *JOINT3_GET_STORED_SILVER1_POS, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ] + common: + <<: *NORMALLY + tolerance: + <<: *NORMAL_TOLERANCE + - step: "left arm" + arm: + joints: [ *JOINT1_READY_GET_STORED_POS, 0.113, -2.561, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ] common: <<: *NORMALLY tolerance: @@ -432,28 +511,37 @@ steps_list: GET_STORED_SILVER2: - step: "ready to get silver from gripper2" arm: - joints: [ *JOINT1_READY_GET_STORED_POS, *JOINT2_GET_STORED_SILVER2_POS, *JOINT3_GET_STORED_SILVER2_POS, *JOINT4_HOME_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ] + joints: [ *JOINT1_READY_GET_STORED_POS, *JOINT2_GET_STORED_SILVER2_POS, *JOINT3_GET_STORED_SILVER2_POS, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ] common: <<: *NORMALLY tolerance: <<: *NORMAL_TOLERANCE - - # - step: "open main gripper" + - step: "open main gripper" + gripper: + <<: *OPEN_MAIN_GRIPPER - step: "store" arm: - joints: [ *JOINT1_GET_STORED_POS, *JOINT2_GET_STORED_SILVER2_POS, *JOINT3_GET_STORED_SILVER2_POS, *JOINT4_HOME_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ] + joints: [ *JOINT1_GET_STORED_POS, *JOINT2_GET_STORED_SILVER2_POS, *JOINT3_GET_STORED_SILVER2_POS, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ] common: <<: *NORMALLY tolerance: <<: *NORMAL_TOLERANCE - - # - step: "close gripper2" + - step: "close gripper2" + gripper: + <<: *CLOSE_S2 - step: "minus stone" stone_num: change: "-s2" - step: "lift arm" arm: - joints: [ *JOINT1_READY_GET_STORED_POS, *JOINT2_GET_STORED_SILVER2_POS, *JOINT3_GET_STORED_SILVER2_POS, *JOINT4_HOME_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ] + joints: [ *JOINT1_READY_GET_STORED_POS, *JOINT2_GET_STORED_SILVER2_POS, *JOINT3_GET_STORED_SILVER2_POS, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ] + common: + <<: *NORMALLY + tolerance: + <<: *NORMAL_TOLERANCE + - step: "left arm" + arm: + joints: [ *JOINT1_READY_GET_STORED_POS, 0.113, -2.561, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ] common: <<: *NORMALLY tolerance: @@ -462,28 +550,37 @@ steps_list: GET_STORED_SILVER3: - step: "ready to get silver from gripper3" arm: - joints: [ *JOINT1_READY_GET_STORED_POS, *JOINT2_GET_STORED_SILVER3_POS, *JOINT3_GET_STORED_SILVER3_POS, *JOINT4_HOME_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ] + joints: [ *JOINT1_READY_GET_STORED_POS, *JOINT2_GET_STORED_SILVER3_POS, *JOINT3_GET_STORED_SILVER3_POS, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ] common: <<: *NORMALLY tolerance: <<: *NORMAL_TOLERANCE - - # - step: "open main gripper" + - step: "open main gripper" + gripper: + <<: *OPEN_MAIN_GRIPPER - step: "store" arm: - joints: [ *JOINT1_GET_STORED_POS, *JOINT2_GET_STORED_SILVER3_POS, *JOINT3_GET_STORED_SILVER3_POS, *JOINT4_HOME_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ] + joints: [ *JOINT1_GET_STORED_POS, *JOINT2_GET_STORED_SILVER3_POS, *JOINT3_GET_STORED_SILVER3_POS, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ] common: <<: *NORMALLY tolerance: <<: *NORMAL_TOLERANCE - - # - step: "close gripper3" + - step: "close gripper3" + gripper: + <<: *CLOSE_S3 - step: "minus stone" stone_num: change: "-s3" - step: "left arm" arm: - joints: [ *JOINT1_READY_GET_STORED_POS, *JOINT2_GET_STORED_SILVER3_POS, *JOINT3_GET_STORED_SILVER3_POS, *JOINT4_HOME_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ] + joints: [ *JOINT1_READY_GET_STORED_POS, *JOINT2_GET_STORED_SILVER3_POS, *JOINT3_GET_STORED_SILVER3_POS, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ] + common: + <<: *NORMALLY + tolerance: + <<: *NORMAL_TOLERANCE + - step: "left arm" + arm: + joints: [ *JOINT1_READY_GET_STORED_POS, 0.113, -2.561, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ] common: <<: *NORMALLY tolerance: @@ -497,7 +594,9 @@ steps_list: <<: *NORMALLY tolerance: <<: *NORMAL_TOLERANCE -# - step: "open main gripper" + - step: "open main gripper" + gripper: + <<: *OPEN_MAIN_GRIPPER - step: "get stored gold" arm: joints: [ *JOINT1_GET_STORED_GOLD_POS, *JOINT2_GET_STORED_GOLD_POS, *JOINT3_GET_STORED_GOLD_POS, *JOINT4_R90_POS, *JOINT5_GET_STORED_GOLD_POS, *JOINT6_HOME_POS] @@ -505,7 +604,9 @@ steps_list: <<: *NORMALLY tolerance: <<: *NORMAL_TOLERANCE -# - step: "close gold gripper + - step: "close gold gripper" + gripper: + <<: *CLOSE_GOLD - step: "add stone" stone_num: change: "+g" @@ -516,7 +617,6 @@ steps_list: <<: *NORMALLY tolerance: <<: *NORMAL_TOLERANCE - - step: "lift up gold" arm: joints: [ *JOINT1_EXCHANGE_POS, *JOINT2_GET_STORED_GOLD_POS, *JOINT3_READY_GET_STORED_GOLD_POS, *JOINT4_R90_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ] @@ -531,126 +631,133 @@ steps_list: target: 1.57 - step: "ready get small island silver" arm: - joints: [ *JOINT1_READY_GET_SMALL_POS, *JOINT2_GET_SMALL_ISLAND_POS, *JOINT3_GET_SMALL_ISLAND_POS, *JOINT4_HOME_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ] + joints: [ *JOINT1_READY_GET_SMALL_POS, *JOINT2_GET_SMALL_ISLAND_POS, *JOINT3_GET_SMALL_ISLAND_POS, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ] common: <<: *NORMALLY tolerance: <<: *NORMAL_TOLERANCE - - # - step: "open main gripper" - + - step: "open main gripper" + gripper: + <<: *OPEN_MAIN_GRIPPER - step: "get small island silver" arm: - joints: [ *JOINT1_GET_SMALL_POS, *JOINT2_GET_SMALL_ISLAND_POS, *JOINT3_GET_SMALL_ISLAND_POS, *JOINT4_HOME_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ] + joints: [ *JOINT1_GET_SMALL_POS, *JOINT2_GET_SMALL_ISLAND_POS, *JOINT3_GET_SMALL_ISLAND_POS, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ] common: <<: *NORMALLY tolerance: <<: *NORMAL_TOLERANCE - step: "pull up silver" arm: - joints: [ *JOINT1_READY_GET_SMALL_POS, *JOINT2_GET_SMALL_ISLAND_POS, *JOINT3_GET_SMALL_ISLAND_POS, *JOINT4_HOME_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ] + joints: [ *JOINT1_READY_GET_SMALL_POS, *JOINT2_GET_SMALL_ISLAND_POS, *JOINT3_GET_SMALL_ISLAND_POS, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ] common: <<: *NORMALLY tolerance: <<: *NORMAL_TOLERANCE - - step: "ready to store" arm: - joints: [ *JOINT1_READY_GET_STORED_POS, *JOINT2_GET_STORED_SILVER2_POS, *JOINT3_GET_STORED_SILVER2_POS, *JOINT4_HOME_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ] + joints: [ *JOINT1_READY_GET_STORED_POS, *JOINT2_GET_STORED_SILVER2_POS, *JOINT3_GET_STORED_SILVER2_POS, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ] common: <<: *NORMALLY tolerance: <<: *NORMAL_TOLERANCE - - # - step: "open gripper2" + - step: "open gripper2" + gripper: + <<: *OPEN_S2 - step: "store" arm: - joints: [ *JOINT1_GET_STORED_POS, *JOINT2_GET_STORED_SILVER2_POS, *JOINT3_GET_STORED_SILVER2_POS, *JOINT4_HOME_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ] + joints: [ *JOINT1_GET_STORED_POS, *JOINT2_GET_STORED_SILVER2_POS, *JOINT3_GET_STORED_SILVER2_POS, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ] common: <<: *NORMALLY tolerance: <<: *NORMAL_TOLERANCE - # - step: "close main gripper" - + - step: "close main gripper" + gripper: + <<: *CLOSE_MAIN_GRIPPER - step: "left arm" arm: - joints: [ *JOINT1_READY_GET_STORED_POS, *JOINT2_GET_STORED_SILVER2_POS, *JOINT3_GET_STORED_SILVER2_POS, *JOINT4_HOME_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ] + joints: [ *JOINT1_READY_GET_STORED_POS, *JOINT2_GET_STORED_SILVER2_POS, *JOINT3_GET_STORED_SILVER2_POS, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ] common: <<: *NORMALLY tolerance: <<: *NORMAL_TOLERANCE - ###################### 2nd silver + + ############################################# 2nd silver + - step: "ready get small island silver" arm: - joints: [ *JOINT1_READY_GET_SMALL_POS, *JOINT2_GET_SMALL_ISLAND_POS, *JOINT3_GET_SMALL_ISLAND_POS, *JOINT4_HOME_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ] + joints: [ *JOINT1_READY_GET_SMALL_POS, *JOINT2_GET_SMALL_ISLAND_POS, *JOINT3_GET_SMALL_ISLAND_POS, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ] common: <<: *NORMALLY tolerance: <<: *NORMAL_TOLERANCE - - # - step: "open main gripper" + - step: "open main gripper" + gripper: + <<: *OPEN_MAIN_GRIPPER - step: "get small island silver" arm: - joints: [ *JOINT1_GET_SMALL_POS, *JOINT2_GET_SMALL_ISLAND_POS, *JOINT3_GET_SMALL_ISLAND_POS, *JOINT4_HOME_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ] + joints: [ *JOINT1_GET_SMALL_POS, *JOINT2_GET_SMALL_ISLAND_POS, *JOINT3_GET_SMALL_ISLAND_POS, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ] common: <<: *NORMALLY tolerance: <<: *NORMAL_TOLERANCE - step: "pull up silver" arm: - joints: [ *JOINT1_READY_GET_SMALL_POS, *JOINT2_GET_SMALL_ISLAND_POS, *JOINT3_GET_SMALL_ISLAND_POS, *JOINT4_HOME_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ] + joints: [ *JOINT1_READY_GET_SMALL_POS, *JOINT2_GET_SMALL_ISLAND_POS, *JOINT3_GET_SMALL_ISLAND_POS, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ] common: <<: *NORMALLY tolerance: <<: *NORMAL_TOLERANCE - - step: "ready to store" arm: - joints: [ *JOINT1_READY_GET_STORED_POS, *JOINT2_GET_STORED_SILVER3_POS, *JOINT3_GET_STORED_SILVER3_POS, *JOINT4_HOME_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ] + joints: [ *JOINT1_READY_GET_STORED_POS, *JOINT2_GET_STORED_SILVER3_POS, *JOINT3_GET_STORED_SILVER3_POS, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ] common: <<: *NORMALLY tolerance: <<: *NORMAL_TOLERANCE - - # - step: "open gripper3" + - step: "open gripper3" + gripper: + <<: *OPEN_S3 - step: "store" arm: - joints: [ *JOINT1_GET_STORED_POS, *JOINT2_GET_STORED_SILVER3_POS, *JOINT3_GET_STORED_SILVER3_POS, *JOINT4_HOME_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ] + joints: [ *JOINT1_GET_STORED_POS, *JOINT2_GET_STORED_SILVER3_POS, *JOINT3_GET_STORED_SILVER3_POS, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ] common: <<: *NORMALLY tolerance: <<: *NORMAL_TOLERANCE - # - step: "close main gripper" - + - step: "close main gripper" + gripper: + <<: *CLOSE_MAIN_GRIPPER - step: "left arm" arm: - joints: [ *JOINT1_READY_GET_STORED_POS, *JOINT2_GET_STORED_SILVER3_POS, *JOINT3_GET_STORED_SILVER3_POS, *JOINT4_HOME_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ] + joints: [ *JOINT1_READY_GET_STORED_POS, *JOINT2_GET_STORED_SILVER3_POS, *JOINT3_GET_STORED_SILVER3_POS, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ] common: <<: *NORMALLY tolerance: <<: *NORMAL_TOLERANCE - ############ 3rd silver + + ####################################################### 3rd silver + - step: "ready get small island silver" arm: - joints: [ *JOINT1_READY_GET_SMALL_POS, *JOINT2_GET_SMALL_ISLAND_POS, *JOINT3_GET_SMALL_ISLAND_POS, *JOINT4_HOME_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ] + joints: [ *JOINT1_READY_GET_SMALL_POS, *JOINT2_GET_SMALL_ISLAND_POS, *JOINT3_GET_SMALL_ISLAND_POS, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ] common: <<: *NORMALLY tolerance: <<: *NORMAL_TOLERANCE - - # - step: "open main gripper" - + - step: "open main gripper" + gripper: + <<: *OPEN_MAIN_GRIPPER - step: "get small island silver" arm: - joints: [ *JOINT1_GET_SMALL_POS, *JOINT2_GET_SMALL_ISLAND_POS, *JOINT3_GET_SMALL_ISLAND_POS, *JOINT4_HOME_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ] + joints: [ *JOINT1_GET_SMALL_POS, *JOINT2_GET_SMALL_ISLAND_POS, *JOINT3_GET_SMALL_ISLAND_POS, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ] common: <<: *NORMALLY tolerance: <<: *NORMAL_TOLERANCE - step: "pull up silver" arm: - joints: [ *JOINT1_READY_GET_SMALL_POS, *JOINT2_GET_SMALL_ISLAND_POS, *JOINT3_GET_SMALL_ISLAND_POS, *JOINT4_HOME_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ] + joints: [ *JOINT1_READY_GET_SMALL_POS, *JOINT2_GET_SMALL_ISLAND_POS, *JOINT3_GET_SMALL_ISLAND_POS, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ] common: <<: *NORMALLY tolerance: @@ -666,8 +773,16 @@ steps_list: - step: "pusher back" silver_pusher: target: 0. - delay: 0.1 -# - step: "open gripper" + delay: 1 + - step: "open gripper" + gripper: + <<: *OPEN_S1 + - step: "open gripper" + gripper: + <<: *OPEN_S2 + - step: "open gripper" + gripper: + <<: *OPEN_S3 - step: "get silver" silver_pusher: target: -0.046 @@ -706,10 +821,12 @@ steps_list: - step: "lifter down" gold_lifter: target: 0 -# - step: "open gold gripper" + - step: "open gold gripper" + gripper: + <<: *OPEN_GOLD - step: "get gold" gold_pusher: - target: -0.51 + target: -0.45 delay: 0.5 - step: "lift gold" gold_lifter: @@ -719,6 +836,7 @@ steps_list: gold_pusher: target: 0 delay: 0.5 +# - step: "chassis left" - step: "lifter down" gold_lifter: target: 0 From 35c127d70148deabe8b1c00e8d9bcb9d2bb4ea01 Mon Sep 17 00:00:00 2001 From: 2194555 <3631676002@qq.com> Date: Sat, 20 Jul 2024 03:26:12 +0800 Subject: [PATCH 50/80] Update parameters and step list. --- .../config/engineer_steps_list.yaml | 69 ++++++++++++++++++- 1 file changed, 67 insertions(+), 2 deletions(-) diff --git a/engineer_middleware/config/engineer_steps_list.yaml b/engineer_middleware/config/engineer_steps_list.yaml index 2a50cb9..92a1e58 100644 --- a/engineer_middleware/config/engineer_steps_list.yaml +++ b/engineer_middleware/config/engineer_steps_list.yaml @@ -42,7 +42,7 @@ common: bin_get_stone_left: &JOINT1_BIN_GET_STONE_LEFT 0.2 bin_get_stone_right: &JOINT1_BIN_GET_STONE_RIGHT - 0.1131 + 0.1198 big_ready: &JOINT1_BIG_READY 0.005 big_adjust_mid: &JOINT1_BIG_ADJUST_MID @@ -82,7 +82,7 @@ common: bin_get_stone: &JOINT2_BIN_GET_STONE 0.13 back_bin_position: &JOINT2_BACK_BIN_POSITION - 0.06651 + 0.06451 avoid_collision: &JOINT2_AVOID_COLLISION 0.1 big_ready: &JOINT2_BIG_READY @@ -258,6 +258,12 @@ common: 0.230 small_island: &LIFTER_SMALL_ISLAND 0.05 + gl_test1: &GL_TEST1 + 0.10 + gl_test2: &GL_TEST2 + 0.15 + gl_test3: &GL_TEST3 + 0.20 gripper: open_gripper: &OPEN_GRIPPER pin: 0 @@ -399,6 +405,34 @@ steps_list: - step: "gimbal lifter tallest" gimbal_lifter: target: *TALLEST_POS + LEFT_EXCHANGE: + - step: "gimbal look front" + gimbal: + <<: *FRONT_POS + - step: "move arm to exchange pos" + arm: + joints: [*JOINT1_DOWN_POSITION, *JOINT2_EXCHANGE_POSITION, *JOINT3_MID_POSITION, *JOINT4_MID_POSITION, *JOINT5_L90_POSITION, *JOINT6_UP_POSITION, *JOINT7_MID_POSITION] + common: + <<: *NORMALLY + tolerance: + <<: *NORMAL_TOLERANCE + - step: "gimbal lifter tallest" + gimbal_lifter: + target: *TALLEST_POS + RIGHT_EXCHANGE: + - step: "gimbal look front" + gimbal: + <<: *FRONT_POS + - step: "move arm to exchange pos" + arm: + joints: [*JOINT1_DOWN_POSITION, *JOINT2_EXCHANGE_POSITION, *JOINT3_MID_POSITION, *JOINT4_MID_POSITION, *JOINT5_R90_POSITION, *JOINT6_UP_POSITION, *JOINT7_MID_POSITION] + common: + <<: *NORMALLY + tolerance: + <<: *NORMAL_TOLERANCE + - step: "gimbal lifter tallest" + gimbal_lifter: + target: *TALLEST_POS ############ MID ############# @@ -1757,6 +1791,19 @@ steps_list: gimbal_lifter: target: *TALLEST_POS + GIMBAL_TEST1: + - step: "gimbal 1" + gimbal_lifter: + target: *GL_TEST1 + GIMBAL_TEST2: + - step: "gimbal 2" + gimbal_lifter: + target: *GL_TEST2 + GIMBAL_TEST3: + - step: "gimbal 3" + gimbal_lifter: + target: *GL_TEST3 + ############## ORE ROTATOR ############### ORE_ROTATOR_INIT: - step: "ore rotator init pos" @@ -1906,3 +1953,21 @@ steps_list: target_frame: arm common: timeout: 2. + + JOINT2_TEST_F: + - step: "joint2 front" + arm: + joints: ["KEEP",*JOINT2_FURTHEST_POSITION, "KEEP", "KEEP", "KEEP", "KEEP", "KEEP" ] + common: + <<: *QUICKLY + tolerance: + <<: *NORMAL_TOLERANCE + + JOINT2_TEST_B: + - step: "joint2 front" + arm: + joints: [ "KEEP",*JOINT2_BACK_POSITION, "KEEP", "KEEP", "KEEP", "KEEP", "KEEP" ] + common: + <<: *QUICKLY + tolerance: + <<: *NORMAL_TOLERANCE From 60416368383617ee234806fc56e740dd8d7f6358 Mon Sep 17 00:00:00 2001 From: cc0h <7921166012@qq.com> Date: Sat, 20 Jul 2024 21:22:38 +0800 Subject: [PATCH 51/80] Record. --- .../config/engineer2_steps_list.yaml | 160 ++++++++++++++++-- 1 file changed, 144 insertions(+), 16 deletions(-) diff --git a/engineer_middleware/config/engineer2_steps_list.yaml b/engineer_middleware/config/engineer2_steps_list.yaml index de2a86e..0acc4b7 100644 --- a/engineer_middleware/config/engineer2_steps_list.yaml +++ b/engineer_middleware/config/engineer2_steps_list.yaml @@ -29,7 +29,7 @@ common: down: &JOINT1_DOWN_POS 0.0000001 up: &JOINT1_UP_POS - 0.579 + 0.545 ready_get_ground: &JOINT1_READY_GET_GROUND_POS 0.5 get_ground: &JOINT1_GET_GROUND_POS @@ -41,7 +41,7 @@ common: get_small: &JOINT1_GET_SMALL_POS 0.3 ready_get_stored: &JOINT1_READY_GET_STORED_POS - 0.545 + 0.54 get_stored: &JOINT1_GET_STORED_POS 0.5 get_stored_gold: &JOINT1_GET_STORED_GOLD_POS @@ -108,6 +108,8 @@ common: joint5: home: &JOINT5_HOME_POS 0.00001 + down_45: &JOINT5_DOWN_45 + 0.77 down_90: &JOINT5_DOWN_90 1.57 get_stored_gold: &JOINT5_GET_STORED_GOLD_POS @@ -167,7 +169,7 @@ common: <<: *NORMAL_TOLERANCE lv4_exchange: &LV4_EXCHANGE - joints: [ *JOINT1_EXCHANGE_POS, *JOINT2_EXCHANGE_POS, *JOINT3_HOME_POS, *JOINT4_ZERO_POS, *JOINT5_HOME_POS, *JOINT6_HOME_POS ] + joints: [ *JOINT1_EXCHANGE_POS, *JOINT2_R_EXCHANGE_POS, *JOINT3_R_EXCHANGE_POS, *JOINT4_ZERO_POS, *JOINT5_DOWN_45, *JOINT6_HOME_POS ] common: <<: *NORMALLY tolerance: @@ -187,6 +189,30 @@ common: tolerance: <<: *NORMAL_TOLERANCE + gimbal: + right_pos: &RIGHT_POS + frame: base_link + position: [ 0.001 , -3., 0. ] + left_pos: &LEFT_POS + frame: base_link + position: [ 0.001, 3., 0. ] + front_pos: &FRONT_POS + frame: base_link + position: [ 2., 0.001, 0. ] + follow_ee: &FOLLOW_EE + frame: link5 + position: [ 0.001, 0.001, 0. ] + + chassis_forward: &CHASSIS_FORWARD + frame: base_link + position: [ 0.1, 0. ] + yaw: 0.0 + chassis_tolerance_position: 0.1 + chassis_tolerance_angular: 0.2 + common: + timeout: 2. + + gripper: open_main_gripper: &OPEN_MAIN_GRIPPER pin: 0 @@ -227,6 +253,21 @@ common: steps_list: + GIMBAL_FRONT: + - step: "gimbal look front" + gimbal: + <<: *FRONT_POS + + GIMBAL_RIGHT: + - step: "gimbal look right" + gimbal: + <<: *RIGHT_POS + + GIMBAL_FOLLOW_EE: + - step: "gimbal follow ee" + gimbal: + <<: *FOLLOW_EE + MIDDLE_PITCH_UP: - step: "middld pitch up" middle_pitch: @@ -310,6 +351,9 @@ steps_list: - step: "silver rotator up" silver_rotator: target: 1.57 + - step: "open silver gripper" + gripper: + <<: *OPEN_S_GRIPPER - step: "ready to store" arm: joints: [*JOINT1_READY_GET_STORED_POS, *JOINT2_GET_STORED_SILVER1_POS, *JOINT3_GET_STORED_SILVER1_POS, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS] @@ -352,6 +396,9 @@ steps_list: - step: "silver rotator up" silver_rotator: target: 1.57 + - step: "open silver gripper" + gripper: + <<: *OPEN_S_GRIPPER - step: "ready to store" arm: joints: [ *JOINT1_READY_GET_STORED_POS, *JOINT2_GET_STORED_SILVER2_POS, *JOINT3_GET_STORED_SILVER2_POS, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ] @@ -394,6 +441,9 @@ steps_list: - step: "silver rotator up" silver_rotator: target: 1.57 + - step: "open silver gripper" + gripper: + <<: *OPEN_S_GRIPPER - step: "ready to store" arm: joints: [ *JOINT1_READY_GET_STORED_POS, *JOINT2_GET_STORED_SILVER3_POS, *JOINT3_GET_STORED_SILVER3_POS, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ] @@ -629,6 +679,9 @@ steps_list: - step: "silver rotator up" silver_rotator: target: 1.57 + - step: "open silver gripper" + gripper: + <<: *OPEN_S_GRIPPER - step: "ready get small island silver" arm: joints: [ *JOINT1_READY_GET_SMALL_POS, *JOINT2_GET_SMALL_ISLAND_POS, *JOINT3_GET_SMALL_ISLAND_POS, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ] @@ -639,6 +692,7 @@ steps_list: - step: "open main gripper" gripper: <<: *OPEN_MAIN_GRIPPER + ARM_THREE_SILVER0: - step: "get small island silver" arm: joints: [ *JOINT1_GET_SMALL_POS, *JOINT2_GET_SMALL_ISLAND_POS, *JOINT3_GET_SMALL_ISLAND_POS, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ] @@ -764,16 +818,25 @@ steps_list: <<: *NORMAL_TOLERANCE GRIPPER_THREE_SILVER: +# - step: "gimbal look back" + - step: "open gripper" + gripper: + <<: *OPEN_S1 + - step: "open gripper" + gripper: + <<: *OPEN_S2 + - step: "open gripper" + gripper: + <<: *OPEN_S3 - step: "rotate down" silver_rotator: target: 0.0001 - - step: "lifter down" + - step: "lifter ready" silver_lifter: - target: 0. + target: 0.07 - step: "pusher back" silver_pusher: - target: 0. - delay: 1 + target: -0.05 - step: "open gripper" gripper: <<: *OPEN_S1 @@ -783,27 +846,41 @@ steps_list: - step: "open gripper" gripper: <<: *OPEN_S3 + - step: "open silver gripper" + gripper: + <<: *OPEN_S_GRIPPER + GRIPPER_THREE_SILVER0: - step: "get silver" silver_pusher: - target: -0.046 - delay: 0.5 + target: -0.03 + delay: 0.3 - step: "pusher back little" silver_pusher: - target: -0.04 - delay: 0.5 + target: -0.03 + delay: 0.1 +# - step: "rotate up" +# silver_rotator: +# target: 0.02 +# delay: 0.5 - step: "lifter up" silver_lifter: - target: 0.18 - - step: "rotate up" - silver_rotator: - target: 1.57 + target: 0.2 delay: 0.5 - step: "pusher back" silver_pusher: target: 0. + delay: 0.1 + - step: "rotate up" + silver_rotator: + target: 1.64 + delay: 0.5 - step: "lifter down" silver_lifter: - target: 0. + target: 0.05 + delay: 0.15 + - step: "lifter down" + silver_lifter: + target: 0.0 - step: "add stone1" stone_num: change: "+s1" @@ -813,6 +890,7 @@ steps_list: - step: "add stone1" stone_num: change: "+s3" +# - step: "gimbal look front" MID_BIG_ISLAND: - step: "pusher back" @@ -824,6 +902,7 @@ steps_list: - step: "open gold gripper" gripper: <<: *OPEN_GOLD + MID_BIG_ISLAND0: - step: "get gold" gold_pusher: target: -0.45 @@ -843,3 +922,52 @@ steps_list: - step: "add stone" stone_num: change: "+g" + OPEN_MAIN: + - step: "main open" + gripper: + <<: *OPEN_MAIN_GRIPPER + CLOSE_MAIN: + - step: "main close" + gripper: + <<: *CLOSE_MAIN_GRIPPER + OPEN_S1: + - step: "s1 open" + gripper: + <<: *OPEN_S1 + CLOSE_S1: + - step: "s1 c" + gripper: + <<: *CLOSE_S1 + OPEN_S2: + - step: "s2 open" + gripper: + <<: *OPEN_S2 + + CLOSE_S2: + - step: "s2 close" + gripper: + <<: *CLOSE_S2 + OPEN_S3: + - step: "s3 open" + gripper: + <<: *OPEN_S3 + CLSOE_S3: + - step: "s3 close" + gripper: + <<: *CLOSE_S3 + OPEN_G: + - step: "g1 open" + gripper: + <<: *OPEN_GOLD + CLOSE_G: + - step: "g1 close" + gripper: + <<: *CLOSE_GOLD + OPEN_SG: + - step: "g1 close" + gripper: + <<: *OPEN_S_GRIPPER + CLOSE_SG: + - step: "g1 close" + gripper: + <<: *CLOSE_S_GRIPPER From ba32d30838d575c9ac6ca7bd89bd4eae99a52690 Mon Sep 17 00:00:00 2001 From: cc0h <7921166012@qq.com> Date: Mon, 22 Jul 2024 21:05:01 +0800 Subject: [PATCH 52/80] Add get side gold motion. --- .../config/engineer2_steps_list.yaml | 58 ++++++++++++++++--- 1 file changed, 51 insertions(+), 7 deletions(-) diff --git a/engineer_middleware/config/engineer2_steps_list.yaml b/engineer_middleware/config/engineer2_steps_list.yaml index 0acc4b7..9242e42 100644 --- a/engineer_middleware/config/engineer2_steps_list.yaml +++ b/engineer_middleware/config/engineer2_steps_list.yaml @@ -159,6 +159,8 @@ common: 1.57 normal: &MIDDLE_PITCH_NORMAL 0.0 + highest: &MIDDLE_PITCH_HIGHEST + -0.1 arm: home: &HOME @@ -269,7 +271,7 @@ steps_list: <<: *FOLLOW_EE MIDDLE_PITCH_UP: - - step: "middld pitch up" + - step: "middle pitch up" middle_pitch: target: *MIDDLE_PITCH_NORMAL HOME: @@ -896,25 +898,27 @@ steps_list: - step: "pusher back" gold_pusher: target: 0. - - step: "lifter down" + - step: "lifter ready" gold_lifter: - target: 0 + target: 0.015 - step: "open gold gripper" gripper: <<: *OPEN_GOLD MID_BIG_ISLAND0: - step: "get gold" gold_pusher: - target: -0.45 + target: -0.443 delay: 0.5 + MID_BIG_ISLAND00: - step: "lift gold" gold_lifter: - target: 0.05 + target: 0.08 delay: 0.5 + MID_BIG_ISLAND000: - step: "pull back gold" gold_pusher: target: 0 - delay: 0.5 + delay: 0.6 # - step: "chassis left" - step: "lifter down" gold_lifter: @@ -922,6 +926,46 @@ steps_list: - step: "add stone" stone_num: change: "+g" + + SIDE_BIG_ISLAND: + - step: "middle pitch up" + middle_pitch: + target: *MIDDLE_PITCH_HIGHEST + - step: "arm ready" + arm: + joints: [ 0.037,0.929,-1.963,-1.474,-1.434,-1.528 ] + common: + <<: *NORMALLY + tolerance: + <<: *NORMAL_TOLERANCE + - step: "open main gripper" + gripper: + <<: *OPEN_MAIN_GRIPPER + SIDE_BIG_ISLAND0: + - step: "arm get gold" + arm: + joints: [ 0.037,0.093,-1.38,-1.474,-1.571,-1.528 ] + common: + <<: *NORMALLY + tolerance: + <<: *NORMAL_TOLERANCE + #SIDE_BIG_ISLAND00: + - step: "arm lift gold" + arm: + joints: [ 0.127,0.093,-1.38,-1.474,-1.571,-1.528 ] + common: + <<: *NORMALLY + tolerance: + <<: *NORMAL_TOLERANCE + #SIDE_BIG_ISLAND000: + - step: "arm pull back gold" + arm: + joints: [ 0.127,1.04,-1.99,-1.474,-1.321,-1.528 ] + common: + <<: *NORMALLY + tolerance: + <<: *NORMAL_TOLERANCE + OPEN_MAIN: - step: "main open" gripper: @@ -951,7 +995,7 @@ steps_list: - step: "s3 open" gripper: <<: *OPEN_S3 - CLSOE_S3: + CLOSE_S3: - step: "s3 close" gripper: <<: *CLOSE_S3 From 45e65eb3323dbf67350acaef5e1ba5197beb4e8d Mon Sep 17 00:00:00 2001 From: cc0h <7921166012@qq.com> Date: Mon, 22 Jul 2024 21:05:41 +0800 Subject: [PATCH 53/80] Enable rm manual in sim.launch. --- engineer_middleware/launch/sim.launch | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/engineer_middleware/launch/sim.launch b/engineer_middleware/launch/sim.launch index 7eebf7e..c8057e2 100644 --- a/engineer_middleware/launch/sim.launch +++ b/engineer_middleware/launch/sim.launch @@ -4,7 +4,7 @@ - + From f082714aaae50ac8879f538a7d24f58edc98b1a4 Mon Sep 17 00:00:00 2001 From: cc0h <7921166012@qq.com> Date: Wed, 24 Jul 2024 05:13:26 +0800 Subject: [PATCH 54/80] Modify arm get small island and store to silver gripper motion. --- .../config/engineer2_steps_list.yaml | 325 +++++++++--------- 1 file changed, 163 insertions(+), 162 deletions(-) diff --git a/engineer_middleware/config/engineer2_steps_list.yaml b/engineer_middleware/config/engineer2_steps_list.yaml index 9242e42..fc7c752 100644 --- a/engineer_middleware/config/engineer2_steps_list.yaml +++ b/engineer_middleware/config/engineer2_steps_list.yaml @@ -19,17 +19,13 @@ common: normal_tolerance: &NORMAL_TOLERANCE tolerance_joints: [ 0.012, 0.02, 0.015, 0.1, 0.2, 0.15, 0.07 ] bigger_tolerance: &BIGGER_TOLERANCE - tolerance_joints: [ 0.015, 0.015, 0.02, 0.35, 0.2, 0.2, 0.1 ] + tolerance_joints: [ 0.015, 0.015, 0.03, 0.35, 0.2, 0.2, 0.1 ] max_tolerance: &MAX_TOLERANCE tolerance_joints: [ 0.03, 0.03, 0.03, 0.3, 0.3, 0.3 , 0.3] joint1: home: &JOINT1_HOME_POS 0.1 - down: &JOINT1_DOWN_POS - 0.0000001 - up: &JOINT1_UP_POS - 0.545 ready_get_ground: &JOINT1_READY_GET_GROUND_POS 0.5 get_ground: &JOINT1_GET_GROUND_POS @@ -37,40 +33,50 @@ common: exchange: &JOINT1_EXCHANGE_POS 0.35 ready_get_small: &JOINT1_READY_GET_SMALL_POS - 0.4 + 0.42 get_small: &JOINT1_GET_SMALL_POS 0.3 ready_get_stored: &JOINT1_READY_GET_STORED_POS 0.54 get_stored: &JOINT1_GET_STORED_POS - 0.5 + 0.478 get_stored_gold: &JOINT1_GET_STORED_GOLD_POS - 0.029 + 0.05 + ready_store: &JOINT1_READY_STORE_POS + 0.44 + store: &JOINT1_STORE_POS + 0.34 joint2: home: &JOINT2_HOME_POS 1.0 - zero: &JOINT2_ZERO_POS - 0.0000001 exchange: &JOINT2_EXCHANGE_POS -1.57 l_exchange: &JOINT2_L_EXCHANGE_POS -0.984 r_exchange: &JOINT2_R_EXCHANGE_POS -2.156 - get_small_island: &JOINT2_GET_SMALL_ISLAND_POS - 0.068 - get_stored_silver3: &JOINT2_GET_STORED_SILVER3_POS - -2.12 - get_stored_silver2: &JOINT2_GET_STORED_SILVER2_POS - -2.967 - get_stored_silver1: &JOINT2_GET_STORED_SILVER1_POS - -3.75 - ready_get_stored_gold: &JOINT2_READY_GET_STORED_GOLD_POS - -1.2 + get_small_island_l: &JOINT2_GET_SMALL_ISLAND_L_POS + -0.098 + get_small_island_m: &JOINT2_GET_SMALL_ISLAND_M_POS + -1.101 + get_small_island_r: &JOINT2_GET_SMALL_ISLAND_R_POS + -2.052 + store_to_s1: &JOINT2_STORE_TO_S1_POS + -3.22 + store_to_s2: &JOINT2_STORE_TO_S2_POS + -2.342 + store_to_s3: &JOINT2_STORE_TO_S3_POS + -1.813 + get_stored_silver3: &JOINT2_GET_STORED_S3_POS + -2.135 + get_stored_silver2: &JOINT2_GET_STORED_S2_POS + -2.941 + get_stored_silver1: &JOINT2_GET_STORED_S1_POS + -3.84 get_stored_gold: &JOINT2_GET_STORED_GOLD_POS - -1.32 + -1.062 joint3: home: &JOINT3_HOME_POS @@ -81,18 +87,28 @@ common: -1.165 r_exchange: &JOINT3_R_EXCHANGE_POS 1.165 - get_small_island: &JOINT3_GET_SMALL_ISLAND_POS - -2.18 + get_small_island_l: &JOINT3_GET_SMALL_ISLAND_L_POS + -1.948 + get_small_island_m: &JOINT3_GET_SMALL_ISLAND_M_POS + -1.833 + get_small_island_r: &JOINT3_GET_SMALL_ISLAND_R_POS + -0.815 + store_to_s1: &JOINT3_STORE_TO_S1_POS + -1.024 + store_to_s2: &JOINT3_STORE_TO_S2_POS + -1.605 + store_to_s3: &JOINT3_STORE_TO_S3_POS + -1.306 get_stored_silver3: &JOINT3_GET_STORED_SILVER3_POS - -1.788 + -1.512 get_stored_silver2: &JOINT3_GET_STORED_SILVER2_POS - -1.483 + -1.209 get_stored_silver1: &JOINT3_GET_STORED_SILVER1_POS - 0.00001 + -0.187 ready_get_stored_gold: &JOINT3_READY_GET_STORED_GOLD_POS - -1.962 + -1.67 get_stored_gold: &JOINT3_GET_STORED_GOLD_POS - -2.181 + -1.815 joint4: home: &JOINT4_HOME_POS @@ -113,46 +129,27 @@ common: down_90: &JOINT5_DOWN_90 1.57 get_stored_gold: &JOINT5_GET_STORED_GOLD_POS - 1.193 + 1.586 + store_to_s1: &JOINT5_STORE_TO_S1_POS + -0.227 + store_to_s2: &JOINT5_STORE_TO_S2_POS + -0.522 + store_to_s3: &JOINT5_STORE_TO_S3_POS + -1.368 joint6: home: &JOINT6_HOME_POS 0.0 l_90: &JOINT6_L_90 - 1.57 + 3.14 r_90: &JOINT6_R_90 - -1.57 - - silver_lift: - home: &SILVER_LIFTER_HOME_POS - 0.0 - lift: &SILVER_LIFTER_LIFT_POS - 0.0 - - silver_push: - home: &SILVER_PUSHER_HOME_POS - 0.0 - get_silver: &SILVER_PUSHER_GET_SILVER_POS - 0.0 - pull_back_little: &SILVER_PUSHER_PULL_BACK_LITTLE_POS - 0.0 - - silver_rotate: - home: &SILVER_ROTATOR_HOME_POS - 0.0 - 90: &SILVER_ROTATOR_90_POS - 1.57 - - gold_push: - home: &GOLD_PUSHER_HOME_POS - 0.0 - get_gold: &GOLD_PUSHER_GET_GOLD_POS - 0.0 - gold_lift: - home: &GOLD_LIFTER_HOME_POS - 0.0 - lift_gold: &GOLD_LIFTER_LIFT_GOLD_POS - 0.0 + -3.14 + get_small3: &JOINT6_SMALL3 + 4.387 + get_small2: &JOINT6_SMALL2 + 4.387 + get_small1: &JOINT6_SMALL1 +# 2.7 middle_pitch: ground_stone: &MIDDLE_PITCH_GROUND_STONE @@ -201,6 +198,9 @@ common: front_pos: &FRONT_POS frame: base_link position: [ 2., 0.001, 0. ] + back_pos: &BACK_POS + frame: base_link + position: [ -2., 0.001, 0. ] follow_ee: &FOLLOW_EE frame: link5 position: [ 0.001, 0.001, 0. ] @@ -326,7 +326,7 @@ steps_list: GET_SMALL_ISLAND: - step: "ready get small island silver" arm: - joints: [*JOINT1_READY_GET_SMALL_POS, *JOINT2_GET_SMALL_ISLAND_POS, *JOINT3_GET_SMALL_ISLAND_POS, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS] + joints: [*JOINT1_READY_GET_SMALL_POS, *JOINT2_GET_SMALL_ISLAND_L_POS, *JOINT3_GET_SMALL_ISLAND_L_POS, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_SMALL1] common: <<: *NORMALLY tolerance: @@ -336,14 +336,14 @@ steps_list: <<: *OPEN_MAIN_GRIPPER - step: "get small island silver" arm: - joints: [ *JOINT1_GET_SMALL_POS, *JOINT2_GET_SMALL_ISLAND_POS, *JOINT3_GET_SMALL_ISLAND_POS, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ] + joints: [ *JOINT1_GET_SMALL_POS, *JOINT2_GET_SMALL_ISLAND_L_POS, *JOINT3_GET_SMALL_ISLAND_L_POS, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_SMALL1 ] common: <<: *NORMALLY tolerance: <<: *NORMAL_TOLERANCE - step: "pull up silver" arm: - joints: [ *JOINT1_READY_GET_SMALL_POS, *JOINT2_GET_SMALL_ISLAND_POS, *JOINT3_GET_SMALL_ISLAND_POS, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ] + joints: [ *JOINT1_READY_GET_SMALL_POS, *JOINT2_GET_SMALL_ISLAND_L_POS, *JOINT3_GET_SMALL_ISLAND_L_POS, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_SMALL1 ] common: <<: *NORMALLY tolerance: @@ -356,19 +356,20 @@ steps_list: - step: "open silver gripper" gripper: <<: *OPEN_S_GRIPPER + + - step: "open gripper1" + gripper: + <<: *OPEN_S1 - step: "ready to store" arm: - joints: [*JOINT1_READY_GET_STORED_POS, *JOINT2_GET_STORED_SILVER1_POS, *JOINT3_GET_STORED_SILVER1_POS, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS] + joints: [*JOINT1_READY_STORE_POS, *JOINT2_STORE_TO_S1_POS, *JOINT3_STORE_TO_S1_POS, *JOINT4_L90_POS, *JOINT5_STORE_TO_S1_POS, *JOINT6_HOME_POS] common: <<: *NORMALLY tolerance: <<: *NORMAL_TOLERANCE - - step: "open gripper1" - gripper: - <<: *OPEN_S1 - step: "store" arm: - joints: [ *JOINT1_GET_STORED_POS, *JOINT2_GET_STORED_SILVER1_POS, *JOINT3_GET_STORED_SILVER1_POS, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ] + joints: [ *JOINT1_STORE_POS, *JOINT2_STORE_TO_S1_POS, *JOINT3_STORE_TO_S1_POS, *JOINT4_L90_POS, *JOINT5_STORE_TO_S1_POS, *JOINT6_HOME_POS ] common: <<: *NORMALLY tolerance: @@ -376,19 +377,19 @@ steps_list: - step: "close main gripper" gripper: <<: *CLOSE_MAIN_GRIPPER - - step: "add stone" - stone_num: - change: "+s1" +# - step: "add stone" +# stone_num: +# change: "+s1" - step: "lift up arm" arm: - joints: [ *JOINT1_READY_GET_STORED_POS, *JOINT2_GET_STORED_SILVER1_POS, *JOINT3_GET_STORED_SILVER1_POS, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ] + joints: [ *JOINT1_READY_STORE_POS, *JOINT2_STORE_TO_S1_POS, *JOINT3_STORE_TO_S1_POS, *JOINT4_L90_POS, *JOINT5_STORE_TO_S1_POS, *JOINT6_HOME_POS ] common: <<: *NORMALLY tolerance: <<: *NORMAL_TOLERANCE - step: "left arm" arm: - joints: [ *JOINT1_READY_GET_STORED_POS, 0.113, -2.561, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ] + joints: [ *JOINT1_READY_GET_STORED_POS, 0.113, -2.063, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ] common: <<: *NORMALLY tolerance: @@ -401,19 +402,19 @@ steps_list: - step: "open silver gripper" gripper: <<: *OPEN_S_GRIPPER + - step: "open gripper2" + gripper: + <<: *OPEN_S2 - step: "ready to store" arm: - joints: [ *JOINT1_READY_GET_STORED_POS, *JOINT2_GET_STORED_SILVER2_POS, *JOINT3_GET_STORED_SILVER2_POS, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ] + joints: [ *JOINT1_READY_STORE_POS, *JOINT2_STORE_TO_S2_POS, *JOINT3_STORE_TO_S2_POS, *JOINT4_L90_POS, *JOINT5_STORE_TO_S2_POS, *JOINT6_HOME_POS ] common: <<: *NORMALLY tolerance: <<: *NORMAL_TOLERANCE - - step: "open gripper2" - gripper: - <<: *OPEN_S2 - step: "store" arm: - joints: [ *JOINT1_GET_STORED_POS, *JOINT2_GET_STORED_SILVER2_POS, *JOINT3_GET_STORED_SILVER2_POS, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ] + joints: [ *JOINT1_STORE_POS, *JOINT2_STORE_TO_S2_POS, *JOINT3_STORE_TO_S2_POS, *JOINT4_L90_POS, *JOINT5_STORE_TO_S2_POS, *JOINT6_HOME_POS ] common: <<: *NORMALLY tolerance: @@ -421,19 +422,19 @@ steps_list: - step: "close main gripper" gripper: <<: *CLOSE_MAIN_GRIPPER - - step: "add stone" - stone_num: - change: "+s2" +# - step: "add stone" +# stone_num: +# change: "+s2" - step: "lift up arm" arm: - joints: [ *JOINT1_READY_GET_STORED_POS, *JOINT2_GET_STORED_SILVER2_POS, *JOINT3_GET_STORED_SILVER2_POS, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ] + joints: [ *JOINT1_READY_STORE_POS, *JOINT2_STORE_TO_S2_POS, *JOINT3_STORE_TO_S2_POS, *JOINT4_L90_POS, *JOINT5_STORE_TO_S2_POS, *JOINT6_HOME_POS ] common: <<: *NORMALLY tolerance: <<: *NORMAL_TOLERANCE - step: "left arm" arm: - joints: [ *JOINT1_READY_GET_STORED_POS, 0.113, -2.561, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ] + joints: [ *JOINT1_READY_GET_STORED_POS, 0.113, -2.063, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ] common: <<: *NORMALLY tolerance: @@ -446,41 +447,39 @@ steps_list: - step: "open silver gripper" gripper: <<: *OPEN_S_GRIPPER + - step: "open gripper3" + gripper: + <<: *OPEN_S3 - step: "ready to store" arm: - joints: [ *JOINT1_READY_GET_STORED_POS, *JOINT2_GET_STORED_SILVER3_POS, *JOINT3_GET_STORED_SILVER3_POS, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ] + joints: [ *JOINT1_READY_STORE_POS, *JOINT2_STORE_TO_S3_POS, *JOINT3_STORE_TO_S3_POS, *JOINT4_L90_POS, *JOINT5_STORE_TO_S3_POS, *JOINT6_HOME_POS ] common: <<: *NORMALLY tolerance: <<: *NORMAL_TOLERANCE - - - step: "open gripper3" - gripper: - <<: *OPEN_S3 - step: "store" arm: - joints: [ *JOINT1_GET_STORED_POS, *JOINT2_GET_STORED_SILVER3_POS, *JOINT3_GET_STORED_SILVER3_POS, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ] + joints: [ *JOINT1_STORE_POS, *JOINT2_STORE_TO_S3_POS, *JOINT3_STORE_TO_S3_POS, *JOINT4_L90_POS, *JOINT5_STORE_TO_S3_POS, *JOINT6_HOME_POS ] common: <<: *NORMALLY tolerance: <<: *NORMAL_TOLERANCE - - step: "close main gripper" gripper: <<: *CLOSE_MAIN_GRIPPER - - step: "add stone" - stone_num: - change: "+s3" +# - step: "add stone" +# stone_num: +# change: "+s3" - step: "lift up arm" arm: - joints: [ *JOINT1_READY_GET_STORED_POS, *JOINT2_GET_STORED_SILVER3_POS, *JOINT3_GET_STORED_SILVER3_POS, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ] + joints: [ *JOINT1_READY_STORE_POS, *JOINT2_STORE_TO_S3_POS, *JOINT3_STORE_TO_S3_POS, *JOINT4_L90_POS, *JOINT5_STORE_TO_S3_POS, *JOINT6_HOME_POS ] common: <<: *NORMALLY tolerance: <<: *NORMAL_TOLERANCE - step: "left arm" arm: - joints: [ *JOINT1_READY_GET_STORED_POS, 0.113, -2.561, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ] + joints: [ *JOINT1_READY_GET_STORED_POS, 0.113, -2.063, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ] common: <<: *NORMALLY tolerance: @@ -523,7 +522,7 @@ steps_list: GET_STORED_SILVER1: - step: "ready to get silver from gripper1" arm: - joints: [*JOINT1_READY_GET_STORED_POS, *JOINT2_GET_STORED_SILVER1_POS, *JOINT3_GET_STORED_SILVER1_POS, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS] + joints: [*JOINT1_READY_GET_STORED_POS, *JOINT2_GET_STORED_S1_POS, *JOINT3_GET_STORED_SILVER1_POS, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS] common: <<: *NORMALLY tolerance: @@ -534,7 +533,7 @@ steps_list: <<: *OPEN_MAIN_GRIPPER - step: "get silver" arm: - joints: [ *JOINT1_GET_STORED_POS, *JOINT2_GET_STORED_SILVER1_POS, *JOINT3_GET_STORED_SILVER1_POS, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ] + joints: [ *JOINT1_GET_STORED_POS, *JOINT2_GET_STORED_S1_POS, *JOINT3_GET_STORED_SILVER1_POS, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ] common: <<: *NORMALLY tolerance: @@ -542,19 +541,19 @@ steps_list: - step: "close gripper1" gripper: <<: *CLOSE_S1 - - step: "minus stone" - stone_num: - change: "-s1" +# - step: "minus stone" +# stone_num: +# change: "-s1" - step: "lift arm" arm: - joints: [ *JOINT1_READY_GET_STORED_POS, *JOINT2_GET_STORED_SILVER1_POS, *JOINT3_GET_STORED_SILVER1_POS, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ] + joints: [ *JOINT1_READY_GET_STORED_POS, *JOINT2_GET_STORED_S1_POS, *JOINT3_GET_STORED_SILVER1_POS, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ] common: <<: *NORMALLY tolerance: <<: *NORMAL_TOLERANCE - step: "left arm" arm: - joints: [ *JOINT1_READY_GET_STORED_POS, 0.113, -2.561, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ] + joints: [ *JOINT1_READY_GET_STORED_POS, 0.113, -2.063, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ] common: <<: *NORMALLY tolerance: @@ -563,7 +562,7 @@ steps_list: GET_STORED_SILVER2: - step: "ready to get silver from gripper2" arm: - joints: [ *JOINT1_READY_GET_STORED_POS, *JOINT2_GET_STORED_SILVER2_POS, *JOINT3_GET_STORED_SILVER2_POS, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ] + joints: [ *JOINT1_READY_GET_STORED_POS, *JOINT2_GET_STORED_S2_POS, *JOINT3_GET_STORED_SILVER2_POS, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ] common: <<: *NORMALLY tolerance: @@ -573,7 +572,7 @@ steps_list: <<: *OPEN_MAIN_GRIPPER - step: "store" arm: - joints: [ *JOINT1_GET_STORED_POS, *JOINT2_GET_STORED_SILVER2_POS, *JOINT3_GET_STORED_SILVER2_POS, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ] + joints: [ *JOINT1_GET_STORED_POS, *JOINT2_GET_STORED_S2_POS, *JOINT3_GET_STORED_SILVER2_POS, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ] common: <<: *NORMALLY tolerance: @@ -581,28 +580,35 @@ steps_list: - step: "close gripper2" gripper: <<: *CLOSE_S2 - - step: "minus stone" - stone_num: - change: "-s2" +# - step: "minus stone" +# stone_num: +# change: "-s2" - step: "lift arm" arm: - joints: [ *JOINT1_READY_GET_STORED_POS, *JOINT2_GET_STORED_SILVER2_POS, *JOINT3_GET_STORED_SILVER2_POS, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ] + joints: [ *JOINT1_READY_GET_STORED_POS, *JOINT2_GET_STORED_S2_POS, *JOINT3_GET_STORED_SILVER2_POS, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ] common: <<: *NORMALLY tolerance: <<: *NORMAL_TOLERANCE - step: "left arm" arm: - joints: [ *JOINT1_READY_GET_STORED_POS, 0.113, -2.561, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ] + joints: [ *JOINT1_READY_GET_STORED_POS, 0.113, -2.063, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ] common: <<: *NORMALLY tolerance: <<: *NORMAL_TOLERANCE GET_STORED_SILVER3: + - step: "lift arm" + arm: + joints: [ *JOINT1_READY_GET_STORED_POS, 0.113, -2.063, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ] + common: + <<: *NORMALLY + tolerance: + <<: *NORMAL_TOLERANCE - step: "ready to get silver from gripper3" arm: - joints: [ *JOINT1_READY_GET_STORED_POS, *JOINT2_GET_STORED_SILVER3_POS, *JOINT3_GET_STORED_SILVER3_POS, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ] + joints: [ *JOINT1_READY_GET_STORED_POS, *JOINT2_GET_STORED_S3_POS, *JOINT3_GET_STORED_SILVER3_POS, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ] common: <<: *NORMALLY tolerance: @@ -610,9 +616,9 @@ steps_list: - step: "open main gripper" gripper: <<: *OPEN_MAIN_GRIPPER - - step: "store" + - step: "get ore" arm: - joints: [ *JOINT1_GET_STORED_POS, *JOINT2_GET_STORED_SILVER3_POS, *JOINT3_GET_STORED_SILVER3_POS, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ] + joints: [ *JOINT1_GET_STORED_POS, *JOINT2_GET_STORED_S3_POS, *JOINT3_GET_STORED_SILVER3_POS, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ] common: <<: *NORMALLY tolerance: @@ -620,19 +626,19 @@ steps_list: - step: "close gripper3" gripper: <<: *CLOSE_S3 - - step: "minus stone" - stone_num: - change: "-s3" +# - step: "minus stone" +# stone_num: +# change: "-s3" - step: "left arm" arm: - joints: [ *JOINT1_READY_GET_STORED_POS, *JOINT2_GET_STORED_SILVER3_POS, *JOINT3_GET_STORED_SILVER3_POS, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ] + joints: [ *JOINT1_READY_GET_STORED_POS, *JOINT2_GET_STORED_S3_POS, *JOINT3_GET_STORED_SILVER3_POS, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ] common: <<: *NORMALLY tolerance: - <<: *NORMAL_TOLERANCE + <<: *BIGGER_TOLERANCE - step: "left arm" arm: - joints: [ *JOINT1_READY_GET_STORED_POS, 0.113, -2.561, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ] + joints: [ *JOINT1_READY_GET_STORED_POS, 0.113, -2.063, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ] common: <<: *NORMALLY tolerance: @@ -655,20 +661,20 @@ steps_list: common: <<: *NORMALLY tolerance: - <<: *NORMAL_TOLERANCE + <<: *BIGGER_TOLERANCE - step: "close gold gripper" gripper: <<: *CLOSE_GOLD - - step: "add stone" - stone_num: - change: "+g" +# - step: "add stone" +# stone_num: +# change: "+g" - step: "move out gold" arm: joints: [ *JOINT1_GET_STORED_GOLD_POS, *JOINT2_GET_STORED_GOLD_POS, *JOINT3_READY_GET_STORED_GOLD_POS, *JOINT4_R90_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ] common: <<: *NORMALLY tolerance: - <<: *NORMAL_TOLERANCE + <<: *BIGGER_TOLERANCE - step: "lift up gold" arm: joints: [ *JOINT1_EXCHANGE_POS, *JOINT2_GET_STORED_GOLD_POS, *JOINT3_READY_GET_STORED_GOLD_POS, *JOINT4_R90_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ] @@ -684,9 +690,9 @@ steps_list: - step: "open silver gripper" gripper: <<: *OPEN_S_GRIPPER - - step: "ready get small island silver" + - step: "ready get small island L silver" arm: - joints: [ *JOINT1_READY_GET_SMALL_POS, *JOINT2_GET_SMALL_ISLAND_POS, *JOINT3_GET_SMALL_ISLAND_POS, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ] + joints: [ *JOINT1_READY_GET_SMALL_POS, *JOINT2_GET_SMALL_ISLAND_L_POS, *JOINT3_GET_SMALL_ISLAND_L_POS, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_SMALL1 ] common: <<: *NORMALLY tolerance: @@ -697,31 +703,28 @@ steps_list: ARM_THREE_SILVER0: - step: "get small island silver" arm: - joints: [ *JOINT1_GET_SMALL_POS, *JOINT2_GET_SMALL_ISLAND_POS, *JOINT3_GET_SMALL_ISLAND_POS, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ] + joints: [ *JOINT1_GET_SMALL_POS, *JOINT2_GET_SMALL_ISLAND_L_POS, *JOINT3_GET_SMALL_ISLAND_L_POS, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_SMALL1 ] common: <<: *NORMALLY tolerance: <<: *NORMAL_TOLERANCE - step: "pull up silver" arm: - joints: [ *JOINT1_READY_GET_SMALL_POS, *JOINT2_GET_SMALL_ISLAND_POS, *JOINT3_GET_SMALL_ISLAND_POS, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ] + joints: [ *JOINT1_READY_GET_SMALL_POS, *JOINT2_GET_SMALL_ISLAND_L_POS, *JOINT3_GET_SMALL_ISLAND_L_POS, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_SMALL1 ] common: <<: *NORMALLY tolerance: <<: *NORMAL_TOLERANCE - step: "ready to store" arm: - joints: [ *JOINT1_READY_GET_STORED_POS, *JOINT2_GET_STORED_SILVER2_POS, *JOINT3_GET_STORED_SILVER2_POS, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ] + joints: [ *JOINT1_READY_STORE_POS, *JOINT2_STORE_TO_S2_POS, *JOINT3_STORE_TO_S2_POS, *JOINT4_L90_POS, *JOINT5_STORE_TO_S2_POS, *JOINT6_HOME_POS ] common: <<: *NORMALLY tolerance: <<: *NORMAL_TOLERANCE - - step: "open gripper2" - gripper: - <<: *OPEN_S2 - step: "store" arm: - joints: [ *JOINT1_GET_STORED_POS, *JOINT2_GET_STORED_SILVER2_POS, *JOINT3_GET_STORED_SILVER2_POS, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ] + joints: [ *JOINT1_STORE_POS, *JOINT2_STORE_TO_S2_POS, *JOINT3_STORE_TO_S2_POS, *JOINT4_L90_POS, *JOINT5_STORE_TO_S2_POS, *JOINT6_HOME_POS ] common: <<: *NORMALLY tolerance: @@ -729,9 +732,12 @@ steps_list: - step: "close main gripper" gripper: <<: *CLOSE_MAIN_GRIPPER - - step: "left arm" + # - step: "add stone" + # stone_num: + # change: "+s2" + - step: "lift up arm" arm: - joints: [ *JOINT1_READY_GET_STORED_POS, *JOINT2_GET_STORED_SILVER2_POS, *JOINT3_GET_STORED_SILVER2_POS, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ] + joints: [ *JOINT1_READY_STORE_POS, *JOINT2_STORE_TO_S2_POS, *JOINT3_STORE_TO_S2_POS, *JOINT4_L90_POS, *JOINT5_STORE_TO_S2_POS, *JOINT6_HOME_POS ] common: <<: *NORMALLY tolerance: @@ -739,9 +745,9 @@ steps_list: ############################################# 2nd silver - - step: "ready get small island silver" + - step: "ready get small island M silver" arm: - joints: [ *JOINT1_READY_GET_SMALL_POS, *JOINT2_GET_SMALL_ISLAND_POS, *JOINT3_GET_SMALL_ISLAND_POS, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ] + joints: [ *JOINT1_READY_GET_SMALL_POS, *JOINT2_GET_SMALL_ISLAND_M_POS, *JOINT3_GET_SMALL_ISLAND_M_POS, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_SMALL2 ] common: <<: *NORMALLY tolerance: @@ -752,31 +758,28 @@ steps_list: - step: "get small island silver" arm: - joints: [ *JOINT1_GET_SMALL_POS, *JOINT2_GET_SMALL_ISLAND_POS, *JOINT3_GET_SMALL_ISLAND_POS, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ] + joints: [ *JOINT1_GET_SMALL_POS, *JOINT2_GET_SMALL_ISLAND_M_POS, *JOINT3_GET_SMALL_ISLAND_M_POS, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_SMALL2 ] common: <<: *NORMALLY tolerance: <<: *NORMAL_TOLERANCE - step: "pull up silver" arm: - joints: [ *JOINT1_READY_GET_SMALL_POS, *JOINT2_GET_SMALL_ISLAND_POS, *JOINT3_GET_SMALL_ISLAND_POS, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ] + joints: [ *JOINT1_READY_GET_SMALL_POS, *JOINT2_GET_SMALL_ISLAND_M_POS, *JOINT3_GET_SMALL_ISLAND_M_POS, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_SMALL2 ] common: <<: *NORMALLY tolerance: <<: *NORMAL_TOLERANCE - step: "ready to store" arm: - joints: [ *JOINT1_READY_GET_STORED_POS, *JOINT2_GET_STORED_SILVER3_POS, *JOINT3_GET_STORED_SILVER3_POS, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ] + joints: [ *JOINT1_READY_STORE_POS, *JOINT2_STORE_TO_S3_POS, *JOINT3_STORE_TO_S3_POS, *JOINT4_L90_POS, *JOINT5_STORE_TO_S3_POS, *JOINT6_HOME_POS ] common: <<: *NORMALLY tolerance: <<: *NORMAL_TOLERANCE - - step: "open gripper3" - gripper: - <<: *OPEN_S3 - step: "store" arm: - joints: [ *JOINT1_GET_STORED_POS, *JOINT2_GET_STORED_SILVER3_POS, *JOINT3_GET_STORED_SILVER3_POS, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ] + joints: [ *JOINT1_STORE_POS, *JOINT2_STORE_TO_S3_POS, *JOINT3_STORE_TO_S3_POS, *JOINT4_L90_POS, *JOINT5_STORE_TO_S3_POS, *JOINT6_HOME_POS ] common: <<: *NORMALLY tolerance: @@ -784,9 +787,12 @@ steps_list: - step: "close main gripper" gripper: <<: *CLOSE_MAIN_GRIPPER - - step: "left arm" + # - step: "add stone" + # stone_num: + # change: "+s3" + - step: "lift up arm" arm: - joints: [ *JOINT1_READY_GET_STORED_POS, *JOINT2_GET_STORED_SILVER3_POS, *JOINT3_GET_STORED_SILVER3_POS, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ] + joints: [ *JOINT1_READY_STORE_POS, *JOINT2_STORE_TO_S3_POS, *JOINT3_STORE_TO_S3_POS, *JOINT4_L90_POS, *JOINT5_STORE_TO_S3_POS, *JOINT6_HOME_POS ] common: <<: *NORMALLY tolerance: @@ -796,7 +802,7 @@ steps_list: - step: "ready get small island silver" arm: - joints: [ *JOINT1_READY_GET_SMALL_POS, *JOINT2_GET_SMALL_ISLAND_POS, *JOINT3_GET_SMALL_ISLAND_POS, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ] + joints: [ *JOINT1_READY_GET_SMALL_POS, *JOINT2_GET_SMALL_ISLAND_R_POS, *JOINT3_GET_SMALL_ISLAND_R_POS, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_SMALL3 ] common: <<: *NORMALLY tolerance: @@ -806,14 +812,14 @@ steps_list: <<: *OPEN_MAIN_GRIPPER - step: "get small island silver" arm: - joints: [ *JOINT1_GET_SMALL_POS, *JOINT2_GET_SMALL_ISLAND_POS, *JOINT3_GET_SMALL_ISLAND_POS, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ] + joints: [ *JOINT1_GET_SMALL_POS, *JOINT2_GET_SMALL_ISLAND_R_POS, *JOINT3_GET_SMALL_ISLAND_R_POS, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_SMALL3 ] common: <<: *NORMALLY tolerance: <<: *NORMAL_TOLERANCE - step: "pull up silver" arm: - joints: [ *JOINT1_READY_GET_SMALL_POS, *JOINT2_GET_SMALL_ISLAND_POS, *JOINT3_GET_SMALL_ISLAND_POS, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ] + joints: [ *JOINT1_READY_GET_SMALL_POS, *JOINT2_GET_SMALL_ISLAND_R_POS, *JOINT3_GET_SMALL_ISLAND_R_POS, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_SMALL3 ] common: <<: *NORMALLY tolerance: @@ -838,7 +844,7 @@ steps_list: target: 0.07 - step: "pusher back" silver_pusher: - target: -0.05 + target: -0.00001 - step: "open gripper" gripper: <<: *OPEN_S1 @@ -855,11 +861,11 @@ steps_list: - step: "get silver" silver_pusher: target: -0.03 - delay: 0.3 + delay: 0.5 - step: "pusher back little" silver_pusher: - target: -0.03 - delay: 0.1 + target: -0.015 + delay: 0.2 # - step: "rotate up" # silver_rotator: # target: 0.02 @@ -900,7 +906,7 @@ steps_list: target: 0. - step: "lifter ready" gold_lifter: - target: 0.015 + target: 0.00001 - step: "open gold gripper" gripper: <<: *OPEN_GOLD @@ -909,20 +915,15 @@ steps_list: gold_pusher: target: -0.443 delay: 0.5 - MID_BIG_ISLAND00: - step: "lift gold" gold_lifter: - target: 0.08 + target: 0.06 delay: 0.5 - MID_BIG_ISLAND000: - step: "pull back gold" gold_pusher: - target: 0 + target: -0.054 delay: 0.6 # - step: "chassis left" - - step: "lifter down" - gold_lifter: - target: 0 - step: "add stone" stone_num: change: "+g" From 7c3ab8c041f516d6fe39a5bba843d94b3d158dd7 Mon Sep 17 00:00:00 2001 From: cc0h <7921166012@qq.com> Date: Wed, 24 Jul 2024 05:22:13 +0800 Subject: [PATCH 55/80] Uncomment necessary data. --- engineer_middleware/config/engineer2_steps_list.yaml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/engineer_middleware/config/engineer2_steps_list.yaml b/engineer_middleware/config/engineer2_steps_list.yaml index fc7c752..b0acfa2 100644 --- a/engineer_middleware/config/engineer2_steps_list.yaml +++ b/engineer_middleware/config/engineer2_steps_list.yaml @@ -149,7 +149,7 @@ common: get_small2: &JOINT6_SMALL2 4.387 get_small1: &JOINT6_SMALL1 -# 2.7 + 2.7 middle_pitch: ground_stone: &MIDDLE_PITCH_GROUND_STONE From aaff087b0a85522e6ff1e57438110095e44f09ee Mon Sep 17 00:00:00 2001 From: cc0h <7921166012@qq.com> Date: Thu, 25 Jul 2024 06:36:35 +0800 Subject: [PATCH 56/80] Rewrite major motion. --- .../config/engineer2_steps_list.yaml | 872 ++++++++++++++++-- 1 file changed, 776 insertions(+), 96 deletions(-) diff --git a/engineer_middleware/config/engineer2_steps_list.yaml b/engineer_middleware/config/engineer2_steps_list.yaml index b0acfa2..4266efc 100644 --- a/engineer_middleware/config/engineer2_steps_list.yaml +++ b/engineer_middleware/config/engineer2_steps_list.yaml @@ -66,15 +66,15 @@ common: store_to_s1: &JOINT2_STORE_TO_S1_POS -3.22 store_to_s2: &JOINT2_STORE_TO_S2_POS - -2.342 + -2.364 store_to_s3: &JOINT2_STORE_TO_S3_POS - -1.813 + -1.763 get_stored_silver3: &JOINT2_GET_STORED_S3_POS - -2.135 + -2.055 get_stored_silver2: &JOINT2_GET_STORED_S2_POS - -2.941 + -2.802 get_stored_silver1: &JOINT2_GET_STORED_S1_POS - -3.84 + -3.833 get_stored_gold: &JOINT2_GET_STORED_GOLD_POS -1.062 @@ -96,19 +96,19 @@ common: store_to_s1: &JOINT3_STORE_TO_S1_POS -1.024 store_to_s2: &JOINT3_STORE_TO_S2_POS - -1.605 + -1.723 store_to_s3: &JOINT3_STORE_TO_S3_POS - -1.306 + -1.467 get_stored_silver3: &JOINT3_GET_STORED_SILVER3_POS - -1.512 + -1.741 get_stored_silver2: &JOINT3_GET_STORED_SILVER2_POS - -1.209 + -1.57 get_stored_silver1: &JOINT3_GET_STORED_SILVER1_POS - -0.187 + -0.307 ready_get_stored_gold: &JOINT3_READY_GET_STORED_GOLD_POS -1.67 get_stored_gold: &JOINT3_GET_STORED_GOLD_POS - -1.815 + -1.92 joint4: home: &JOINT4_HOME_POS @@ -131,25 +131,31 @@ common: get_stored_gold: &JOINT5_GET_STORED_GOLD_POS 1.586 store_to_s1: &JOINT5_STORE_TO_S1_POS - -0.227 + -0.215 store_to_s2: &JOINT5_STORE_TO_S2_POS - -0.522 + -0.314 store_to_s3: &JOINT5_STORE_TO_S3_POS - -1.368 + -1.153 joint6: home: &JOINT6_HOME_POS 0.0 l_90: &JOINT6_L_90 - 3.14 + 1.57 r_90: &JOINT6_R_90 - -3.14 + -1.57 get_small3: &JOINT6_SMALL3 4.387 get_small2: &JOINT6_SMALL2 4.387 get_small1: &JOINT6_SMALL1 2.7 + get_s3: &JOINT6_GET_S3 + -0.9 + get_s2: &JOINT6_GET_S2 + -0.346 + get_s1: &JOINT6_GET_S1 + -0.65 middle_pitch: ground_stone: &MIDDLE_PITCH_GROUND_STONE @@ -191,19 +197,19 @@ common: gimbal: right_pos: &RIGHT_POS frame: base_link - position: [ 0.001 , -3., 0. ] + position: [ 0.00 , -3., 0. ] left_pos: &LEFT_POS frame: base_link position: [ 0.001, 3., 0. ] front_pos: &FRONT_POS frame: base_link - position: [ 2., 0.001, 0. ] + position: [ 3., -0.3, 0. ] back_pos: &BACK_POS frame: base_link - position: [ -2., 0.001, 0. ] + position: [ -3., -0.3, 0. ] follow_ee: &FOLLOW_EE frame: link5 - position: [ 0.001, 0.001, 0. ] + position: [ 0.00, 0.00, 0. ] chassis_forward: &CHASSIS_FORWARD frame: base_link @@ -214,6 +220,15 @@ common: common: timeout: 2. + chassis_fmove_left: &CHASSIS_MOVE_LEFT + frame: base_link + position: [ 0.0, 0.2 ] + yaw: 0.0 + chassis_tolerance_position: 0.1 + chassis_tolerance_angular: 0.2 + common: + timeout: 2. + gripper: open_main_gripper: &OPEN_MAIN_GRIPPER @@ -255,17 +270,22 @@ common: steps_list: - GIMBAL_FRONT: + GIMBAL_F: - step: "gimbal look front" gimbal: <<: *FRONT_POS - GIMBAL_RIGHT: + GIMBAL_R: - step: "gimbal look right" gimbal: <<: *RIGHT_POS - GIMBAL_FOLLOW_EE: + GIMBAL_B: + - step: "gimbal look back" + gimbal: + <<: *BACK_POS + + GIMBAL_EE: - step: "gimbal follow ee" gimbal: <<: *FOLLOW_EE @@ -275,6 +295,27 @@ steps_list: middle_pitch: target: *MIDDLE_PITCH_NORMAL HOME: + - step: "pusher back" + silver_pusher: + target: 0. + - step: "rotate up" + silver_rotator: + target: 0. + - step: "lifter down" + silver_lifter: + target: 0.0 + - step: "get gold" + gold_pusher: + target: 0. + - step: "lift gold" + gold_lifter: + target: 0. + - step: "gimbal look front" + gimbal: + <<: *RIGHT_POS + - step: "gimbal look front" + gimbal: + <<: *FRONT_POS - step: "arm home pos" arm: joints: [*JOINT1_HOME_POS, *JOINT2_HOME_POS, *JOINT3_HOME_POS, *JOINT4_HOME_POS, *JOINT5_HOME_POS, *JOINT6_HOME_POS] @@ -284,6 +325,9 @@ steps_list: <<: *NORMAL_TOLERANCE HOME_WITH_PITCH: + - step: "gimbal look front" + gimbal: + <<: *FRONT_POS - step: "arm home pos" arm: joints: [ *JOINT1_HOME_POS, *JOINT2_HOME_POS, *JOINT3_HOME_POS, *JOINT4_HOME_POS, *JOINT5_HOME_POS, *JOINT6_HOME_POS ] @@ -299,17 +343,24 @@ steps_list: - step: "arm enter exchange pos" arm: <<: *LV4_EXCHANGE -# - step: "gimbal ready" - + - step: "gimbal ready" + gimbal: + <<: *FOLLOW_EE LV5_L_EXCHANGE: - step: "arm enter exchange pos" arm: <<: *LV5_L_EXCHANGE + - step: "gimbal ready" + gimbal: + <<: *FOLLOW_EE LV5_R_EXCHANGE: - step: "arm enter exchange pos" arm: <<: *LV5_R_EXCHANGE + - step: "gimbal ready" + gimbal: + <<: *FOLLOW_EE @@ -324,6 +375,9 @@ steps_list: <<: *CLOSE_S_GRIPPER GET_SMALL_ISLAND: + - step: "gimbal look right" + gimbal: + <<: *RIGHT_POS - step: "ready get small island silver" arm: joints: [*JOINT1_READY_GET_SMALL_POS, *JOINT2_GET_SMALL_ISLAND_L_POS, *JOINT3_GET_SMALL_ISLAND_L_POS, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_SMALL1] @@ -377,9 +431,9 @@ steps_list: - step: "close main gripper" gripper: <<: *CLOSE_MAIN_GRIPPER -# - step: "add stone" -# stone_num: -# change: "+s1" + - step: "add stone" + stone_num: + change: "+s1" - step: "lift up arm" arm: joints: [ *JOINT1_READY_STORE_POS, *JOINT2_STORE_TO_S1_POS, *JOINT3_STORE_TO_S1_POS, *JOINT4_L90_POS, *JOINT5_STORE_TO_S1_POS, *JOINT6_HOME_POS ] @@ -422,9 +476,9 @@ steps_list: - step: "close main gripper" gripper: <<: *CLOSE_MAIN_GRIPPER -# - step: "add stone" -# stone_num: -# change: "+s2" + - step: "add stone" + stone_num: + change: "+s2" - step: "lift up arm" arm: joints: [ *JOINT1_READY_STORE_POS, *JOINT2_STORE_TO_S2_POS, *JOINT3_STORE_TO_S2_POS, *JOINT4_L90_POS, *JOINT5_STORE_TO_S2_POS, *JOINT6_HOME_POS ] @@ -467,9 +521,9 @@ steps_list: - step: "close main gripper" gripper: <<: *CLOSE_MAIN_GRIPPER -# - step: "add stone" -# stone_num: -# change: "+s3" + - step: "add stone" + stone_num: + change: "+s3" - step: "lift up arm" arm: joints: [ *JOINT1_READY_STORE_POS, *JOINT2_STORE_TO_S3_POS, *JOINT3_STORE_TO_S3_POS, *JOINT4_L90_POS, *JOINT5_STORE_TO_S3_POS, *JOINT6_HOME_POS ] @@ -522,7 +576,7 @@ steps_list: GET_STORED_SILVER1: - step: "ready to get silver from gripper1" arm: - joints: [*JOINT1_READY_GET_STORED_POS, *JOINT2_GET_STORED_S1_POS, *JOINT3_GET_STORED_SILVER1_POS, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS] + joints: [*JOINT1_READY_GET_STORED_POS, *JOINT2_GET_STORED_S1_POS, *JOINT3_GET_STORED_SILVER1_POS, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_GET_S1] common: <<: *NORMALLY tolerance: @@ -533,7 +587,7 @@ steps_list: <<: *OPEN_MAIN_GRIPPER - step: "get silver" arm: - joints: [ *JOINT1_GET_STORED_POS, *JOINT2_GET_STORED_S1_POS, *JOINT3_GET_STORED_SILVER1_POS, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ] + joints: [ *JOINT1_GET_STORED_POS, *JOINT2_GET_STORED_S1_POS, *JOINT3_GET_STORED_SILVER1_POS, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_GET_S1 ] common: <<: *NORMALLY tolerance: @@ -541,12 +595,12 @@ steps_list: - step: "close gripper1" gripper: <<: *CLOSE_S1 -# - step: "minus stone" -# stone_num: -# change: "-s1" + - step: "minus stone" + stone_num: + change: "-s1" - step: "lift arm" arm: - joints: [ *JOINT1_READY_GET_STORED_POS, *JOINT2_GET_STORED_S1_POS, *JOINT3_GET_STORED_SILVER1_POS, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ] + joints: [ *JOINT1_READY_GET_STORED_POS, *JOINT2_GET_STORED_S1_POS, *JOINT3_GET_STORED_SILVER1_POS, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_GET_S1 ] common: <<: *NORMALLY tolerance: @@ -559,10 +613,10 @@ steps_list: tolerance: <<: *NORMAL_TOLERANCE - GET_STORED_SILVER2: + GET_STORED_SILVER2: &GET_STORED_S2 - step: "ready to get silver from gripper2" arm: - joints: [ *JOINT1_READY_GET_STORED_POS, *JOINT2_GET_STORED_S2_POS, *JOINT3_GET_STORED_SILVER2_POS, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ] + joints: [ *JOINT1_READY_GET_STORED_POS, *JOINT2_GET_STORED_S2_POS, *JOINT3_GET_STORED_SILVER2_POS, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_GET_S2 ] common: <<: *NORMALLY tolerance: @@ -572,7 +626,7 @@ steps_list: <<: *OPEN_MAIN_GRIPPER - step: "store" arm: - joints: [ *JOINT1_GET_STORED_POS, *JOINT2_GET_STORED_S2_POS, *JOINT3_GET_STORED_SILVER2_POS, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ] + joints: [ *JOINT1_GET_STORED_POS, *JOINT2_GET_STORED_S2_POS, *JOINT3_GET_STORED_SILVER2_POS, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_GET_S2 ] common: <<: *NORMALLY tolerance: @@ -580,35 +634,35 @@ steps_list: - step: "close gripper2" gripper: <<: *CLOSE_S2 -# - step: "minus stone" -# stone_num: -# change: "-s2" + - step: "minus stone" + stone_num: + change: "-s2" - step: "lift arm" arm: - joints: [ *JOINT1_READY_GET_STORED_POS, *JOINT2_GET_STORED_S2_POS, *JOINT3_GET_STORED_SILVER2_POS, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ] + joints: [ *JOINT1_READY_GET_STORED_POS, *JOINT2_GET_STORED_S2_POS, *JOINT3_GET_STORED_SILVER2_POS, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_GET_S2 ] common: <<: *NORMALLY tolerance: <<: *NORMAL_TOLERANCE - step: "left arm" arm: - joints: [ *JOINT1_READY_GET_STORED_POS, 0.113, -2.063, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ] + joints: [ *JOINT1_READY_GET_STORED_POS, 0.113, -2.063, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_GET_S2 ] common: <<: *NORMALLY tolerance: <<: *NORMAL_TOLERANCE - GET_STORED_SILVER3: + GET_STORED_SILVER3: &GET_STORED_S3 - step: "lift arm" arm: - joints: [ *JOINT1_READY_GET_STORED_POS, 0.113, -2.063, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ] + joints: [ *JOINT1_READY_GET_STORED_POS, 0.113, -2.063, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_GET_S3 ] common: <<: *NORMALLY tolerance: <<: *NORMAL_TOLERANCE - step: "ready to get silver from gripper3" arm: - joints: [ *JOINT1_READY_GET_STORED_POS, *JOINT2_GET_STORED_S3_POS, *JOINT3_GET_STORED_SILVER3_POS, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ] + joints: [ *JOINT1_READY_GET_STORED_POS, *JOINT2_GET_STORED_S3_POS, *JOINT3_GET_STORED_SILVER3_POS, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_GET_S3 ] common: <<: *NORMALLY tolerance: @@ -618,7 +672,7 @@ steps_list: <<: *OPEN_MAIN_GRIPPER - step: "get ore" arm: - joints: [ *JOINT1_GET_STORED_POS, *JOINT2_GET_STORED_S3_POS, *JOINT3_GET_STORED_SILVER3_POS, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ] + joints: [ *JOINT1_GET_STORED_POS, *JOINT2_GET_STORED_S3_POS, *JOINT3_GET_STORED_SILVER3_POS, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_GET_S3 ] common: <<: *NORMALLY tolerance: @@ -626,19 +680,19 @@ steps_list: - step: "close gripper3" gripper: <<: *CLOSE_S3 -# - step: "minus stone" -# stone_num: -# change: "-s3" + - step: "minus stone" + stone_num: + change: "-s3" - step: "left arm" arm: - joints: [ *JOINT1_READY_GET_STORED_POS, *JOINT2_GET_STORED_S3_POS, *JOINT3_GET_STORED_SILVER3_POS, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ] + joints: [ *JOINT1_READY_GET_STORED_POS, *JOINT2_GET_STORED_S3_POS, *JOINT3_GET_STORED_SILVER3_POS, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_GET_S3 ] common: <<: *NORMALLY tolerance: <<: *BIGGER_TOLERANCE - step: "left arm" arm: - joints: [ *JOINT1_READY_GET_STORED_POS, 0.113, -2.063, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ] + joints: [ *JOINT1_READY_GET_STORED_POS, 0.113, -2.063, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_GET_S3 ] common: <<: *NORMALLY tolerance: @@ -665,9 +719,9 @@ steps_list: - step: "close gold gripper" gripper: <<: *CLOSE_GOLD -# - step: "add stone" -# stone_num: -# change: "+g" + - step: "add stone" + stone_num: + change: "+g" - step: "move out gold" arm: joints: [ *JOINT1_GET_STORED_GOLD_POS, *JOINT2_GET_STORED_GOLD_POS, *JOINT3_READY_GET_STORED_GOLD_POS, *JOINT4_R90_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ] @@ -684,6 +738,9 @@ steps_list: <<: *NORMAL_TOLERANCE ARM_THREE_SILVER: + - step: "gimbal look right" + gimbal: + <<: *RIGHT_POS - step: "silver rotator up" silver_rotator: target: 1.57 @@ -692,25 +749,28 @@ steps_list: <<: *OPEN_S_GRIPPER - step: "ready get small island L silver" arm: - joints: [ *JOINT1_READY_GET_SMALL_POS, *JOINT2_GET_SMALL_ISLAND_L_POS, *JOINT3_GET_SMALL_ISLAND_L_POS, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_SMALL1 ] + joints: [ *JOINT1_READY_GET_SMALL_POS, *JOINT2_GET_SMALL_ISLAND_R_POS, *JOINT3_GET_SMALL_ISLAND_R_POS, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_SMALL3 ] common: <<: *NORMALLY tolerance: <<: *NORMAL_TOLERANCE + - step: "open s2" + gripper: + <<: *OPEN_S2 - step: "open main gripper" gripper: <<: *OPEN_MAIN_GRIPPER ARM_THREE_SILVER0: - step: "get small island silver" arm: - joints: [ *JOINT1_GET_SMALL_POS, *JOINT2_GET_SMALL_ISLAND_L_POS, *JOINT3_GET_SMALL_ISLAND_L_POS, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_SMALL1 ] + joints: [ *JOINT1_GET_SMALL_POS, *JOINT2_GET_SMALL_ISLAND_R_POS, *JOINT3_GET_SMALL_ISLAND_R_POS, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_SMALL3 ] common: <<: *NORMALLY tolerance: <<: *NORMAL_TOLERANCE - step: "pull up silver" arm: - joints: [ *JOINT1_READY_GET_SMALL_POS, *JOINT2_GET_SMALL_ISLAND_L_POS, *JOINT3_GET_SMALL_ISLAND_L_POS, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_SMALL1 ] + joints: [ *JOINT1_READY_GET_SMALL_POS, *JOINT2_GET_SMALL_ISLAND_R_POS, *JOINT3_GET_SMALL_ISLAND_R_POS, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_SMALL3 ] common: <<: *NORMALLY tolerance: @@ -732,9 +792,9 @@ steps_list: - step: "close main gripper" gripper: <<: *CLOSE_MAIN_GRIPPER - # - step: "add stone" - # stone_num: - # change: "+s2" + - step: "add stone" + stone_num: + change: "+s2" - step: "lift up arm" arm: joints: [ *JOINT1_READY_STORE_POS, *JOINT2_STORE_TO_S2_POS, *JOINT3_STORE_TO_S2_POS, *JOINT4_L90_POS, *JOINT5_STORE_TO_S2_POS, *JOINT6_HOME_POS ] @@ -784,12 +844,15 @@ steps_list: <<: *NORMALLY tolerance: <<: *NORMAL_TOLERANCE + - step: "open s3" + gripper: + <<: *OPEN_S3 - step: "close main gripper" gripper: <<: *CLOSE_MAIN_GRIPPER - # - step: "add stone" - # stone_num: - # change: "+s3" + - step: "add stone" + stone_num: + change: "+s3" - step: "lift up arm" arm: joints: [ *JOINT1_READY_STORE_POS, *JOINT2_STORE_TO_S3_POS, *JOINT3_STORE_TO_S3_POS, *JOINT4_L90_POS, *JOINT5_STORE_TO_S3_POS, *JOINT6_HOME_POS ] @@ -802,7 +865,7 @@ steps_list: - step: "ready get small island silver" arm: - joints: [ *JOINT1_READY_GET_SMALL_POS, *JOINT2_GET_SMALL_ISLAND_R_POS, *JOINT3_GET_SMALL_ISLAND_R_POS, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_SMALL3 ] + joints: [ *JOINT1_READY_GET_SMALL_POS, *JOINT2_GET_SMALL_ISLAND_L_POS, *JOINT3_GET_SMALL_ISLAND_L_POS, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_SMALL1 ] common: <<: *NORMALLY tolerance: @@ -812,21 +875,39 @@ steps_list: <<: *OPEN_MAIN_GRIPPER - step: "get small island silver" arm: - joints: [ *JOINT1_GET_SMALL_POS, *JOINT2_GET_SMALL_ISLAND_R_POS, *JOINT3_GET_SMALL_ISLAND_R_POS, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_SMALL3 ] + joints: [ *JOINT1_GET_SMALL_POS, *JOINT2_GET_SMALL_ISLAND_L_POS, *JOINT3_GET_SMALL_ISLAND_L_POS, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_SMALL1 ] common: <<: *NORMALLY tolerance: <<: *NORMAL_TOLERANCE - step: "pull up silver" arm: - joints: [ *JOINT1_READY_GET_SMALL_POS, *JOINT2_GET_SMALL_ISLAND_R_POS, *JOINT3_GET_SMALL_ISLAND_R_POS, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_SMALL3 ] + joints: [ *JOINT1_READY_GET_SMALL_POS, *JOINT2_GET_SMALL_ISLAND_L_POS, *JOINT3_GET_SMALL_ISLAND_L_POS, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_SMALL1 ] common: <<: *NORMALLY tolerance: <<: *NORMAL_TOLERANCE GRIPPER_THREE_SILVER: -# - step: "gimbal look back" + - step: "open gripper" + gripper: + <<: *OPEN_S1 + - step: "open gripper" + gripper: + <<: *OPEN_S2 + - step: "open gripper" + gripper: + <<: *OPEN_S3 + - step: "lift gimbal up" + arm: + joints: [ *JOINT1_GET_SMALL_POS, *JOINT2_HOME_POS, *JOINT3_ZERO_POS, *JOINT4_HOME_POS, *JOINT5_HOME_POS, *JOINT6_HOME_POS ] + common: + <<: *QUICKLY + tolerance: + <<: *BIGGER_TOLERANCE + - step: "gimbal look right" + gimbal: + <<: *RIGHT_POS - step: "open gripper" gripper: <<: *OPEN_S1 @@ -857,6 +938,9 @@ steps_list: - step: "open silver gripper" gripper: <<: *OPEN_S_GRIPPER + - step: "gimbal look back" + gimbal: + <<: *BACK_POS GRIPPER_THREE_SILVER0: - step: "get silver" silver_pusher: @@ -866,10 +950,6 @@ steps_list: silver_pusher: target: -0.015 delay: 0.2 -# - step: "rotate up" -# silver_rotator: -# target: 0.02 -# delay: 0.5 - step: "lifter up" silver_lifter: target: 0.2 @@ -898,9 +978,24 @@ steps_list: - step: "add stone1" stone_num: change: "+s3" -# - step: "gimbal look front" + - step: "gimbal look right" + gimbal: + <<: *RIGHT_POS + - step: "gimbal look front" + gimbal: + <<: *FRONT_POS + - step: "arm home pos" + arm: + joints: [ *JOINT1_HOME_POS, *JOINT2_HOME_POS, *JOINT3_HOME_POS, *JOINT4_HOME_POS, *JOINT5_HOME_POS, *JOINT6_HOME_POS ] + common: + <<: *NORMALLY + tolerance: + <<: *NORMAL_TOLERANCE MID_BIG_ISLAND: + - step: "gimbal look right" + gimbal: + <<: *RIGHT_POS - step: "pusher back" gold_pusher: target: 0. @@ -915,6 +1010,9 @@ steps_list: gold_pusher: target: -0.443 delay: 0.5 + - step: "gimbal look front" + gimbal: + <<: *FRONT_POS - step: "lift gold" gold_lifter: target: 0.06 @@ -923,12 +1021,18 @@ steps_list: gold_pusher: target: -0.054 delay: 0.6 -# - step: "chassis left" + - step: "chassis left" + chassis: + <<: *CHASSIS_MOVE_LEFT - step: "add stone" stone_num: change: "+g" + SIDE_BIG_ISLAND: + - step: "gimbal look right" + gimbal: + <<: *RIGHT_POS - step: "middle pitch up" middle_pitch: target: *MIDDLE_PITCH_HIGHEST @@ -950,7 +1054,6 @@ steps_list: <<: *NORMALLY tolerance: <<: *NORMAL_TOLERANCE - #SIDE_BIG_ISLAND00: - step: "arm lift gold" arm: joints: [ 0.127,0.093,-1.38,-1.474,-1.571,-1.528 ] @@ -958,7 +1061,6 @@ steps_list: <<: *NORMALLY tolerance: <<: *NORMAL_TOLERANCE - #SIDE_BIG_ISLAND000: - step: "arm pull back gold" arm: joints: [ 0.127,1.04,-1.99,-1.474,-1.321,-1.528 ] @@ -966,53 +1068,631 @@ steps_list: <<: *NORMALLY tolerance: <<: *NORMAL_TOLERANCE + - step: "gimbal look front" + gimbal: + <<: *FRONT_POS + + BOTH_BIG_ISLAND: + - step: "gimbal look right" + gimbal: + <<: *RIGHT_POS + - step: "gimbal look right" + gimbal: + <<: *RIGHT_POS + - step: "pusher back" + gold_pusher: + target: 0. + - step: "lifter ready" + gold_lifter: + target: 0.00001 + - step: "open gold gripper" + gripper: + <<: *OPEN_GOLD + - step: "middle pitch up" + middle_pitch: + target: *MIDDLE_PITCH_HIGHEST + - step: "arm ready" + arm: + joints: [ 0.037,0.929,-1.963,-1.474,-1.434,-1.528 ] + common: + <<: *QUICKLY + tolerance: + <<: *NORMAL_TOLERANCE + - step: "open main gripper" + gripper: + <<: *OPEN_MAIN_GRIPPER + BOTH_BIG_ISLAND0: + - step: "get gold" + gold_pusher: + target: -0.443 + delay: 0.3 + - step: "lift gold" + gold_lifter: + target: 0.06 + - step: "arm get gold" + arm: + joints: [ 0.037,0.093,-1.38,-1.474,-1.571,-1.528 ] + common: + <<: *NORMALLY + tolerance: + <<: *NORMAL_TOLERANCE + - step: "arm lift gold" + arm: + joints: [ 0.127,0.093,-1.38,-1.474,-1.571,-1.528 ] + common: + <<: *NORMALLY + tolerance: + <<: *NORMAL_TOLERANCE + - step: "pull back gold" + gold_pusher: + target: -0.054 + - step: "add stone" + stone_num: + change: "+g" + - step: "gimbal look front" + gimbal: + <<: *FRONT_POS + - step: "arm pull back gold" + arm: + joints: [ 0.127,1.04,-1.99,-1.474,-1.321,-1.528 ] + common: + <<: *NORMALLY + tolerance: + <<: *NORMAL_TOLERANCE + - step: "chassis left" + chassis: + <<: *CHASSIS_MOVE_LEFT - OPEN_MAIN: + OM: - step: "main open" gripper: <<: *OPEN_MAIN_GRIPPER - CLOSE_MAIN: + CM: - step: "main close" gripper: <<: *CLOSE_MAIN_GRIPPER - OPEN_S1: + OS1: - step: "s1 open" gripper: <<: *OPEN_S1 - CLOSE_S1: - - step: "s1 c" + CS1: + - step: "s1 close" gripper: <<: *CLOSE_S1 - OPEN_S2: + OS2: - step: "s2 open" gripper: <<: *OPEN_S2 - - CLOSE_S2: + CS2: - step: "s2 close" gripper: <<: *CLOSE_S2 - OPEN_S3: + OS3: - step: "s3 open" gripper: <<: *OPEN_S3 - CLOSE_S3: + CS3: - step: "s3 close" gripper: <<: *CLOSE_S3 - OPEN_G: + OG: - step: "g1 open" gripper: <<: *OPEN_GOLD - CLOSE_G: + CG: - step: "g1 close" gripper: <<: *CLOSE_GOLD - OPEN_SG: + OSG: - step: "g1 close" gripper: <<: *OPEN_S_GRIPPER - CLOSE_SG: - - step: "g1 close" + CSG: + - step: "close silver gripper" + gripper: + <<: *CLOSE_S_GRIPPER + + LV5_L_S1: + - step: "ready to get silver from gripper1" + arm: + joints: [ *JOINT1_READY_GET_STORED_POS, *JOINT2_GET_STORED_S1_POS, *JOINT3_GET_STORED_SILVER1_POS, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_GET_S1 ] + common: + <<: *NORMALLY + tolerance: + <<: *NORMAL_TOLERANCE + + - step: "open main gripper" + gripper: + <<: *OPEN_MAIN_GRIPPER + - step: "get silver" + arm: + joints: [ *JOINT1_GET_STORED_POS, *JOINT2_GET_STORED_S1_POS, *JOINT3_GET_STORED_SILVER1_POS, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_GET_S1 ] + common: + <<: *NORMALLY + tolerance: + <<: *NORMAL_TOLERANCE + - step: "close gripper1" + gripper: + <<: *CLOSE_S1 + - step: "minus stone" + stone_num: + change: "-s1" + - step: "lift arm" + arm: + joints: [ *JOINT1_READY_GET_STORED_POS, *JOINT2_GET_STORED_S1_POS, *JOINT3_GET_STORED_SILVER1_POS, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_GET_S1 ] + common: + <<: *NORMALLY + tolerance: + <<: *NORMAL_TOLERANCE + - step: "close silver gripper" gripper: <<: *CLOSE_S_GRIPPER + - step: "arm enter exchange pos" + arm: + <<: *LV5_L_EXCHANGE + - step: "gimbal ready" + gimbal: + <<: *FOLLOW_EE + + + + LV5_L_S2: + - step: "ready to get silver from gripper2" + arm: + joints: [ *JOINT1_READY_GET_STORED_POS, *JOINT2_GET_STORED_S2_POS, *JOINT3_GET_STORED_SILVER2_POS, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_GET_S2 ] + common: + <<: *NORMALLY + tolerance: + <<: *NORMAL_TOLERANCE + - step: "open main gripper" + gripper: + <<: *OPEN_MAIN_GRIPPER + - step: "store" + arm: + joints: [ *JOINT1_GET_STORED_POS, *JOINT2_GET_STORED_S2_POS, *JOINT3_GET_STORED_SILVER2_POS, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_GET_S2 ] + common: + <<: *NORMALLY + tolerance: + <<: *NORMAL_TOLERANCE + - step: "close gripper2" + gripper: + <<: *CLOSE_S2 + - step: "minus stone" + stone_num: + change: "-s2" + - step: "lift arm" + arm: + joints: [ *JOINT1_READY_GET_STORED_POS, *JOINT2_GET_STORED_S2_POS, *JOINT3_GET_STORED_SILVER2_POS, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_GET_S2 ] + common: + <<: *NORMALLY + tolerance: + <<: *NORMAL_TOLERANCE + - step: "arm enter exchange pos" + arm: + <<: *LV5_L_EXCHANGE + - step: "gimbal ready" + gimbal: + <<: *FOLLOW_EE + + + + LV5_L_S3: + - step: "lift arm" + arm: + joints: [ *JOINT1_READY_GET_STORED_POS, 0.113, -2.063, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ] + common: + <<: *NORMALLY + tolerance: + <<: *NORMAL_TOLERANCE + - step: "ready to get silver from gripper3" + arm: + joints: [ *JOINT1_READY_GET_STORED_POS, *JOINT2_GET_STORED_S3_POS, *JOINT3_GET_STORED_SILVER3_POS, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_GET_S3 ] + common: + <<: *NORMALLY + tolerance: + <<: *NORMAL_TOLERANCE + - step: "open main gripper" + gripper: + <<: *OPEN_MAIN_GRIPPER + - step: "get ore" + arm: + joints: [ *JOINT1_GET_STORED_POS, *JOINT2_GET_STORED_S3_POS, *JOINT3_GET_STORED_SILVER3_POS, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_GET_S3 ] + common: + <<: *NORMALLY + tolerance: + <<: *NORMAL_TOLERANCE + - step: "close gripper3" + gripper: + <<: *CLOSE_S3 + - step: "minus stone" + stone_num: + change: "-s3" + - step: "left arm" + arm: + joints: [ *JOINT1_READY_GET_STORED_POS, *JOINT2_GET_STORED_S3_POS, *JOINT3_GET_STORED_SILVER3_POS, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_GET_S3 ] + common: + <<: *NORMALLY + tolerance: + <<: *BIGGER_TOLERANCE + - step: "arm enter exchange pos" + arm: + <<: *LV5_L_EXCHANGE + - step: "gimbal ready" + gimbal: + <<: *FOLLOW_EE + LV5_L_G: + - step: "ready to get stored gold" + arm: + joints: [ *JOINT1_GET_STORED_GOLD_POS, *JOINT2_GET_STORED_GOLD_POS, *JOINT3_READY_GET_STORED_GOLD_POS, *JOINT4_R90_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ] + common: + <<: *NORMALLY + tolerance: + <<: *NORMAL_TOLERANCE + - step: "open main gripper" + gripper: + <<: *OPEN_MAIN_GRIPPER + - step: "get stored gold" + arm: + joints: [ *JOINT1_GET_STORED_GOLD_POS, *JOINT2_GET_STORED_GOLD_POS, *JOINT3_GET_STORED_GOLD_POS, *JOINT4_R90_POS, *JOINT5_GET_STORED_GOLD_POS, *JOINT6_HOME_POS ] + common: + <<: *NORMALLY + tolerance: + <<: *BIGGER_TOLERANCE + - step: "close gold gripper" + gripper: + <<: *CLOSE_GOLD + - step: "add stone" + stone_num: + change: "-g" + - step: "move out gold" + arm: + joints: [ *JOINT1_GET_STORED_GOLD_POS, *JOINT2_GET_STORED_GOLD_POS, *JOINT3_READY_GET_STORED_GOLD_POS, *JOINT4_R90_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ] + common: + <<: *NORMALLY + tolerance: + <<: *BIGGER_TOLERANCE + - step: "lift up gold" + arm: + joints: [ *JOINT1_EXCHANGE_POS, *JOINT2_GET_STORED_GOLD_POS, *JOINT3_READY_GET_STORED_GOLD_POS, *JOINT4_R90_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ] + common: + <<: *NORMALLY + tolerance: + <<: *NORMAL_TOLERANCE + - step: "arm enter exchange pos" + arm: + <<: *LV5_L_EXCHANGE + - step: "gimbal ready" + gimbal: + <<: *FOLLOW_EE + LV5_R_S1: + - step: "ready to get silver from gripper1" + arm: + joints: [ *JOINT1_READY_GET_STORED_POS, *JOINT2_GET_STORED_S1_POS, *JOINT3_GET_STORED_SILVER1_POS, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_GET_S1 ] + common: + <<: *NORMALLY + tolerance: + <<: *NORMAL_TOLERANCE + + - step: "open main gripper" + gripper: + <<: *OPEN_MAIN_GRIPPER + - step: "get silver" + arm: + joints: [ *JOINT1_GET_STORED_POS, *JOINT2_GET_STORED_S1_POS, *JOINT3_GET_STORED_SILVER1_POS, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_GET_S1 ] + common: + <<: *NORMALLY + tolerance: + <<: *NORMAL_TOLERANCE + - step: "close gripper1" + gripper: + <<: *CLOSE_S1 + - step: "minus stone" + stone_num: + change: "-s1" + - step: "lift arm" + arm: + joints: [ *JOINT1_READY_GET_STORED_POS, *JOINT2_GET_STORED_S1_POS, *JOINT3_GET_STORED_SILVER1_POS, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_GET_S1 ] + common: + <<: *NORMALLY + tolerance: + <<: *NORMAL_TOLERANCE + - step: "close silver gripper" + gripper: + <<: *CLOSE_S_GRIPPER + - step: "arm enter exchange pos" + arm: + <<: *LV5_R_EXCHANGE + - step: "gimbal ready" + gimbal: + <<: *FOLLOW_EE + LV5_R_S2: + - step: "ready to get silver from gripper2" + arm: + joints: [ *JOINT1_READY_GET_STORED_POS, *JOINT2_GET_STORED_S2_POS, *JOINT3_GET_STORED_SILVER2_POS, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_GET_S2 ] + common: + <<: *NORMALLY + tolerance: + <<: *NORMAL_TOLERANCE + - step: "open main gripper" + gripper: + <<: *OPEN_MAIN_GRIPPER + - step: "store" + arm: + joints: [ *JOINT1_GET_STORED_POS, *JOINT2_GET_STORED_S2_POS, *JOINT3_GET_STORED_SILVER2_POS, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_GET_S2 ] + common: + <<: *NORMALLY + tolerance: + <<: *NORMAL_TOLERANCE + - step: "close gripper2" + gripper: + <<: *CLOSE_S2 + - step: "minus stone" + stone_num: + change: "-s2" + - step: "lift arm" + arm: + joints: [ *JOINT1_READY_GET_STORED_POS, *JOINT2_GET_STORED_S2_POS, *JOINT3_GET_STORED_SILVER2_POS, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_GET_S2 ] + common: + <<: *NORMALLY + tolerance: + <<: *NORMAL_TOLERANCE + - step: "arm enter exchange pos" + arm: + <<: *LV5_R_EXCHANGE + - step: "gimbal ready" + gimbal: + <<: *FOLLOW_EE + LV5_R_S3: + - step: "lift arm" + arm: + joints: [ *JOINT1_READY_GET_STORED_POS, 0.113, -2.063, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ] + common: + <<: *NORMALLY + tolerance: + <<: *NORMAL_TOLERANCE + - step: "ready to get silver from gripper3" + arm: + joints: [ *JOINT1_READY_GET_STORED_POS, *JOINT2_GET_STORED_S3_POS, *JOINT3_GET_STORED_SILVER3_POS, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_GET_S3 ] + common: + <<: *NORMALLY + tolerance: + <<: *NORMAL_TOLERANCE + - step: "open main gripper" + gripper: + <<: *OPEN_MAIN_GRIPPER + - step: "get ore" + arm: + joints: [ *JOINT1_GET_STORED_POS, *JOINT2_GET_STORED_S3_POS, *JOINT3_GET_STORED_SILVER3_POS, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_GET_S3 ] + common: + <<: *NORMALLY + tolerance: + <<: *NORMAL_TOLERANCE + - step: "close gripper3" + gripper: + <<: *CLOSE_S3 + - step: "minus stone" + stone_num: + change: "-s3" + - step: "left arm" + arm: + joints: [ *JOINT1_READY_GET_STORED_POS, *JOINT2_GET_STORED_S3_POS, *JOINT3_GET_STORED_SILVER3_POS, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_GET_S3 ] + common: + <<: *NORMALLY + tolerance: + <<: *BIGGER_TOLERANCE + - step: "arm enter exchange pos" + arm: + <<: *LV5_R_EXCHANGE + - step: "gimbal ready" + gimbal: + <<: *FOLLOW_EE + LV5_R_G: + - step: "ready to get stored gold" + arm: + joints: [ *JOINT1_GET_STORED_GOLD_POS, *JOINT2_GET_STORED_GOLD_POS, *JOINT3_READY_GET_STORED_GOLD_POS, *JOINT4_R90_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ] + common: + <<: *NORMALLY + tolerance: + <<: *NORMAL_TOLERANCE + - step: "open main gripper" + gripper: + <<: *OPEN_MAIN_GRIPPER + - step: "get stored gold" + arm: + joints: [ *JOINT1_GET_STORED_GOLD_POS, *JOINT2_GET_STORED_GOLD_POS, *JOINT3_GET_STORED_GOLD_POS, *JOINT4_R90_POS, *JOINT5_GET_STORED_GOLD_POS, *JOINT6_HOME_POS ] + common: + <<: *NORMALLY + tolerance: + <<: *BIGGER_TOLERANCE + - step: "close gold gripper" + gripper: + <<: *CLOSE_GOLD + - step: "add stone" + stone_num: + change: "-g" + - step: "move out gold" + arm: + joints: [ *JOINT1_GET_STORED_GOLD_POS, *JOINT2_GET_STORED_GOLD_POS, *JOINT3_READY_GET_STORED_GOLD_POS, *JOINT4_R90_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ] + common: + <<: *NORMALLY + tolerance: + <<: *BIGGER_TOLERANCE + - step: "lift up gold" + arm: + joints: [ *JOINT1_EXCHANGE_POS, *JOINT2_GET_STORED_GOLD_POS, *JOINT3_READY_GET_STORED_GOLD_POS, *JOINT4_R90_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ] + common: + <<: *NORMALLY + tolerance: + <<: *NORMAL_TOLERANCE + - step: "arm enter exchange pos" + arm: + <<: *LV5_R_EXCHANGE + - step: "gimbal ready" + gimbal: + <<: *FOLLOW_EE + LV4_S1: + - step: "ready to get silver from gripper1" + arm: + joints: [ *JOINT1_READY_GET_STORED_POS, *JOINT2_GET_STORED_S1_POS, *JOINT3_GET_STORED_SILVER1_POS, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_GET_S1 ] + common: + <<: *NORMALLY + tolerance: + <<: *NORMAL_TOLERANCE + + - step: "open main gripper" + gripper: + <<: *OPEN_MAIN_GRIPPER + - step: "get silver" + arm: + joints: [ *JOINT1_GET_STORED_POS, *JOINT2_GET_STORED_S1_POS, *JOINT3_GET_STORED_SILVER1_POS, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_GET_S1 ] + common: + <<: *NORMALLY + tolerance: + <<: *NORMAL_TOLERANCE + - step: "close gripper1" + gripper: + <<: *CLOSE_S1 + - step: "minus stone" + stone_num: + change: "-s1" + - step: "lift arm" + arm: + joints: [ *JOINT1_READY_GET_STORED_POS, *JOINT2_GET_STORED_S1_POS, *JOINT3_GET_STORED_SILVER1_POS, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_GET_S1 ] + common: + <<: *NORMALLY + tolerance: + <<: *NORMAL_TOLERANCE + - step: "close silver gripper" + gripper: + <<: *CLOSE_S_GRIPPER + - step: "arm enter exchange pos" + arm: + <<: *LV4_EXCHANGE + - step: "gimbal ready" + gimbal: + <<: *FOLLOW_EE + LV4_S2: + - step: "ready to get silver from gripper2" + arm: + joints: [ *JOINT1_READY_GET_STORED_POS, *JOINT2_GET_STORED_S2_POS, *JOINT3_GET_STORED_SILVER2_POS, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_GET_S2 ] + common: + <<: *NORMALLY + tolerance: + <<: *NORMAL_TOLERANCE + - step: "open main gripper" + gripper: + <<: *OPEN_MAIN_GRIPPER + - step: "store" + arm: + joints: [ *JOINT1_GET_STORED_POS, *JOINT2_GET_STORED_S2_POS, *JOINT3_GET_STORED_SILVER2_POS, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_GET_S2 ] + common: + <<: *NORMALLY + tolerance: + <<: *NORMAL_TOLERANCE + - step: "close gripper2" + gripper: + <<: *CLOSE_S2 + - step: "minus stone" + stone_num: + change: "-s2" + - step: "lift arm" + arm: + joints: [ *JOINT1_READY_GET_STORED_POS, *JOINT2_GET_STORED_S2_POS, *JOINT3_GET_STORED_SILVER2_POS, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_GET_S2 ] + common: + <<: *NORMALLY + tolerance: + <<: *NORMAL_TOLERANCE + - step: "arm enter exchange pos" + arm: + <<: *LV4_EXCHANGE + - step: "gimbal ready" + gimbal: + <<: *FOLLOW_EE + LV4_S3: + - step: "lift arm" + arm: + joints: [ *JOINT1_READY_GET_STORED_POS, 0.113, -2.063, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ] + common: + <<: *NORMALLY + tolerance: + <<: *NORMAL_TOLERANCE + - step: "ready to get silver from gripper3" + arm: + joints: [ *JOINT1_READY_GET_STORED_POS, *JOINT2_GET_STORED_S3_POS, *JOINT3_GET_STORED_SILVER3_POS, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_GET_S3 ] + common: + <<: *NORMALLY + tolerance: + <<: *NORMAL_TOLERANCE + - step: "open main gripper" + gripper: + <<: *OPEN_MAIN_GRIPPER + - step: "get ore" + arm: + joints: [ *JOINT1_GET_STORED_POS, *JOINT2_GET_STORED_S3_POS, *JOINT3_GET_STORED_SILVER3_POS, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_GET_S3 ] + common: + <<: *NORMALLY + tolerance: + <<: *NORMAL_TOLERANCE + - step: "close gripper3" + gripper: + <<: *CLOSE_S3 + - step: "minus stone" + stone_num: + change: "-s3" + - step: "left arm" + arm: + joints: [ *JOINT1_READY_GET_STORED_POS, *JOINT2_GET_STORED_S3_POS, *JOINT3_GET_STORED_SILVER3_POS, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_GET_S3 ] + common: + <<: *NORMALLY + tolerance: + <<: *BIGGER_TOLERANCE + - step: "arm enter exchange pos" + arm: + <<: *LV4_EXCHANGE + - step: "gimbal ready" + gimbal: + <<: *FOLLOW_EE + LV4_G: + - step: "ready to get stored gold" + arm: + joints: [ *JOINT1_GET_STORED_GOLD_POS, *JOINT2_GET_STORED_GOLD_POS, *JOINT3_READY_GET_STORED_GOLD_POS, *JOINT4_R90_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ] + common: + <<: *NORMALLY + tolerance: + <<: *NORMAL_TOLERANCE + - step: "open main gripper" + gripper: + <<: *OPEN_MAIN_GRIPPER + - step: "get stored gold" + arm: + joints: [ *JOINT1_GET_STORED_GOLD_POS, *JOINT2_GET_STORED_GOLD_POS, *JOINT3_GET_STORED_GOLD_POS, *JOINT4_R90_POS, *JOINT5_GET_STORED_GOLD_POS, *JOINT6_HOME_POS ] + common: + <<: *NORMALLY + tolerance: + <<: *BIGGER_TOLERANCE + - step: "close gold gripper" + gripper: + <<: *CLOSE_GOLD + - step: "add stone" + stone_num: + change: "-g" + - step: "move out gold" + arm: + joints: [ *JOINT1_GET_STORED_GOLD_POS, *JOINT2_GET_STORED_GOLD_POS, *JOINT3_READY_GET_STORED_GOLD_POS, *JOINT4_R90_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ] + common: + <<: *NORMALLY + tolerance: + <<: *BIGGER_TOLERANCE + - step: "lift up gold" + arm: + joints: [ *JOINT1_EXCHANGE_POS, *JOINT2_GET_STORED_GOLD_POS, *JOINT3_READY_GET_STORED_GOLD_POS, *JOINT4_R90_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ] + common: + <<: *NORMALLY + tolerance: + <<: *NORMAL_TOLERANCE + - step: "arm enter exchange pos" + arm: + <<: *LV4_EXCHANGE + - step: "gimbal ready" + gimbal: + <<: *FOLLOW_EE From 2f41fc8cf25ca367cbe8e578847022c03e1d7391 Mon Sep 17 00:00:00 2001 From: cc0h <7921166012@qq.com> Date: Thu, 25 Jul 2024 23:01:37 +0800 Subject: [PATCH 57/80] Add middle pitch motion before arm move. --- .../config/engineer2_steps_list.yaml | 137 ++++++++++++------ 1 file changed, 93 insertions(+), 44 deletions(-) diff --git a/engineer_middleware/config/engineer2_steps_list.yaml b/engineer_middleware/config/engineer2_steps_list.yaml index 4266efc..ae78c86 100644 --- a/engineer_middleware/config/engineer2_steps_list.yaml +++ b/engineer_middleware/config/engineer2_steps_list.yaml @@ -27,7 +27,7 @@ common: home: &JOINT1_HOME_POS 0.1 ready_get_ground: &JOINT1_READY_GET_GROUND_POS - 0.5 + 0.005 get_ground: &JOINT1_GET_GROUND_POS 0.4 exchange: &JOINT1_EXCHANGE_POS @@ -51,6 +51,8 @@ common: joint2: home: &JOINT2_HOME_POS 1.0 + avoid_controller: &JOINT2_AVOID_CONTROLLER_POS + 0.6 exchange: &JOINT2_EXCHANGE_POS -1.57 l_exchange: &JOINT2_L_EXCHANGE_POS @@ -159,7 +161,7 @@ common: middle_pitch: ground_stone: &MIDDLE_PITCH_GROUND_STONE - 1.57 + 1.3 normal: &MIDDLE_PITCH_NORMAL 0.0 highest: &MIDDLE_PITCH_HIGHEST @@ -208,8 +210,8 @@ common: frame: base_link position: [ -3., -0.3, 0. ] follow_ee: &FOLLOW_EE - frame: link5 - position: [ 0.00, 0.00, 0. ] + frame: link6 + position: [ 0.15, 0.00, 0. ] chassis_forward: &CHASSIS_FORWARD frame: base_link @@ -298,9 +300,6 @@ steps_list: - step: "pusher back" silver_pusher: target: 0. - - step: "rotate up" - silver_rotator: - target: 0. - step: "lifter down" silver_lifter: target: 0.0 @@ -318,11 +317,10 @@ steps_list: <<: *FRONT_POS - step: "arm home pos" arm: - joints: [*JOINT1_HOME_POS, *JOINT2_HOME_POS, *JOINT3_HOME_POS, *JOINT4_HOME_POS, *JOINT5_HOME_POS, *JOINT6_HOME_POS] - common: - <<: *NORMALLY - tolerance: - <<: *NORMAL_TOLERANCE + <<: *HOME + - step: "middle pitch down" + middle_pitch: + target: *MIDDLE_PITCH_GROUND_STONE HOME_WITH_PITCH: - step: "gimbal look front" @@ -330,16 +328,15 @@ steps_list: <<: *FRONT_POS - step: "arm home pos" arm: - joints: [ *JOINT1_HOME_POS, *JOINT2_HOME_POS, *JOINT3_HOME_POS, *JOINT4_HOME_POS, *JOINT5_HOME_POS, *JOINT6_HOME_POS ] - common: - <<: *NORMALLY - tolerance: - <<: *NORMAL_TOLERANCE - - step: "middle pitch up" + <<: *HOME + - step: "middle pitch down" middle_pitch: - target: *MIDDLE_PITCH_NORMAL + target: *MIDDLE_PITCH_GROUND_STONE LV4_EXCHANGE: + - step: "middle pitch up" + middle_pitch: + target: *MIDDLE_PITCH_NORMAL - step: "arm enter exchange pos" arm: <<: *LV4_EXCHANGE @@ -347,6 +344,9 @@ steps_list: gimbal: <<: *FOLLOW_EE LV5_L_EXCHANGE: + - step: "middle pitch up" + middle_pitch: + target: *MIDDLE_PITCH_NORMAL - step: "arm enter exchange pos" arm: <<: *LV5_L_EXCHANGE @@ -355,6 +355,9 @@ steps_list: <<: *FOLLOW_EE LV5_R_EXCHANGE: + - step: "middle pitch up" + middle_pitch: + target: *MIDDLE_PITCH_NORMAL - step: "arm enter exchange pos" arm: <<: *LV5_R_EXCHANGE @@ -362,19 +365,10 @@ steps_list: gimbal: <<: *FOLLOW_EE - - - OPEN_SILVER_GRIPPER: - - step: "open silver gripper" - gripper: - <<: *OPEN_S_GRIPPER - - CLOSE_SILVER_GRIPPER: - - step: "close silver gripper" - gripper: - <<: *CLOSE_S_GRIPPER - GET_SMALL_ISLAND: + - step: "middle pitch up" + middle_pitch: + target: *MIDDLE_PITCH_NORMAL - step: "gimbal look right" gimbal: <<: *RIGHT_POS @@ -542,24 +536,28 @@ steps_list: GROUND_STONE: - step: "arm ready get ground stone" arm: - joints: [*JOINT1_READY_GET_GROUND_POS, *JOINT2_EXCHANGE_POS, *JOINT3_ZERO_POS, *JOINT4_ZERO_POS, *JOINT5_HOME_POS, *JOINT6_HOME_POS] + joints: [*JOINT1_READY_GET_GROUND_POS, *JOINT2_HOME_POS, *JOINT3_ZERO_POS, *JOINT4_HOME_POS, *JOINT5_HOME_POS, *JOINT6_HOME_POS] common: <<: *NORMALLY tolerance: <<: *NORMAL_TOLERANCE - - step: "ready to get ground stone" - middle_pitch: - target: *MIDDLE_PITCH_GROUND_STONE - - step: "open main gripper" - gripper: - <<: *OPEN_MAIN_GRIPPER - - step: "arm down get ground stone" + - step: "gimbal front" + gimbal: + <<: *FRONT_POS + GROUND_STONE0: + - step: "move arm to get ground stone" arm: - joints: [*JOINT1_GET_GROUND_POS, *JOINT2_EXCHANGE_POS, *JOINT3_ZERO_POS, *JOINT4_ZERO_POS, *JOINT5_HOME_POS, *JOINT6_HOME_POS] + joints: [ *JOINT1_READY_GET_GROUND_POS, *JOINT2_HOME_POS, *JOINT3_ZERO_POS, *JOINT4_HOME_POS, *JOINT5_HOME_POS, *JOINT6_HOME_POS ] common: <<: *NORMALLY tolerance: <<: *NORMAL_TOLERANCE + - step: "open main gripper" + gripper: + <<: *OPEN_MAIN_GRIPPER + - step: "ready to get ground stone" + middle_pitch: + target: *MIDDLE_PITCH_GROUND_STONE - step: "arm pull up stone" arm: joints: [*JOINT1_READY_GET_GROUND_POS, *JOINT2_EXCHANGE_POS, *JOINT3_ZERO_POS, *JOINT4_ZERO_POS, *JOINT5_HOME_POS, *JOINT6_HOME_POS] @@ -567,13 +565,13 @@ steps_list: <<: *NORMALLY tolerance: <<: *NORMAL_TOLERANCE - - step: "middle pitch home" - middle_pitch: - target: *MIDDLE_PITCH_NORMAL GET_STORED_SILVER1: + - step: "middle pitch up" + middle_pitch: + target: *MIDDLE_PITCH_NORMAL - step: "ready to get silver from gripper1" arm: joints: [*JOINT1_READY_GET_STORED_POS, *JOINT2_GET_STORED_S1_POS, *JOINT3_GET_STORED_SILVER1_POS, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_GET_S1] @@ -614,6 +612,9 @@ steps_list: <<: *NORMAL_TOLERANCE GET_STORED_SILVER2: &GET_STORED_S2 + - step: "middle pitch up" + middle_pitch: + target: *MIDDLE_PITCH_NORMAL - step: "ready to get silver from gripper2" arm: joints: [ *JOINT1_READY_GET_STORED_POS, *JOINT2_GET_STORED_S2_POS, *JOINT3_GET_STORED_SILVER2_POS, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_GET_S2 ] @@ -653,6 +654,9 @@ steps_list: <<: *NORMAL_TOLERANCE GET_STORED_SILVER3: &GET_STORED_S3 + - step: "middle pitch up" + middle_pitch: + target: *MIDDLE_PITCH_NORMAL - step: "lift arm" arm: joints: [ *JOINT1_READY_GET_STORED_POS, 0.113, -2.063, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_GET_S3 ] @@ -738,6 +742,9 @@ steps_list: <<: *NORMAL_TOLERANCE ARM_THREE_SILVER: + - step: "middle pitch up" + middle_pitch: + target: *MIDDLE_PITCH_NORMAL - step: "gimbal look right" gimbal: <<: *RIGHT_POS @@ -900,7 +907,7 @@ steps_list: <<: *OPEN_S3 - step: "lift gimbal up" arm: - joints: [ *JOINT1_GET_SMALL_POS, *JOINT2_HOME_POS, *JOINT3_ZERO_POS, *JOINT4_HOME_POS, *JOINT5_HOME_POS, *JOINT6_HOME_POS ] + joints: [ *JOINT1_GET_SMALL_POS, *JOINT2_AVOID_CONTROLLER_POS, *JOINT3_HOME_POS, *JOINT4_HOME_POS, *JOINT5_HOME_POS, *JOINT6_HOME_POS ] common: <<: *QUICKLY tolerance: @@ -1030,6 +1037,9 @@ steps_list: SIDE_BIG_ISLAND: + - step: "middle pitch up" + middle_pitch: + target: *MIDDLE_PITCH_NORMAL - step: "gimbal look right" gimbal: <<: *RIGHT_POS @@ -1073,6 +1083,9 @@ steps_list: <<: *FRONT_POS BOTH_BIG_ISLAND: + - step: "middle pitch up" + middle_pitch: + target: *MIDDLE_PITCH_NORMAL - step: "gimbal look right" gimbal: <<: *RIGHT_POS @@ -1193,6 +1206,9 @@ steps_list: <<: *CLOSE_S_GRIPPER LV5_L_S1: + - step: "middle pitch up" + middle_pitch: + target: *MIDDLE_PITCH_NORMAL - step: "ready to get silver from gripper1" arm: joints: [ *JOINT1_READY_GET_STORED_POS, *JOINT2_GET_STORED_S1_POS, *JOINT3_GET_STORED_SILVER1_POS, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_GET_S1 ] @@ -1237,6 +1253,9 @@ steps_list: LV5_L_S2: + - step: "middle pitch up" + middle_pitch: + target: *MIDDLE_PITCH_NORMAL - step: "ready to get silver from gripper2" arm: joints: [ *JOINT1_READY_GET_STORED_POS, *JOINT2_GET_STORED_S2_POS, *JOINT3_GET_STORED_SILVER2_POS, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_GET_S2 ] @@ -1277,6 +1296,9 @@ steps_list: LV5_L_S3: + - step: "middle pitch up" + middle_pitch: + target: *MIDDLE_PITCH_NORMAL - step: "lift arm" arm: joints: [ *JOINT1_READY_GET_STORED_POS, 0.113, -2.063, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ] @@ -1321,6 +1343,9 @@ steps_list: gimbal: <<: *FOLLOW_EE LV5_L_G: + - step: "middle pitch up" + middle_pitch: + target: *MIDDLE_PITCH_NORMAL - step: "ready to get stored gold" arm: joints: [ *JOINT1_GET_STORED_GOLD_POS, *JOINT2_GET_STORED_GOLD_POS, *JOINT3_READY_GET_STORED_GOLD_POS, *JOINT4_R90_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ] @@ -1365,6 +1390,9 @@ steps_list: gimbal: <<: *FOLLOW_EE LV5_R_S1: + - step: "middle pitch up" + middle_pitch: + target: *MIDDLE_PITCH_NORMAL - step: "ready to get silver from gripper1" arm: joints: [ *JOINT1_READY_GET_STORED_POS, *JOINT2_GET_STORED_S1_POS, *JOINT3_GET_STORED_SILVER1_POS, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_GET_S1 ] @@ -1406,6 +1434,9 @@ steps_list: gimbal: <<: *FOLLOW_EE LV5_R_S2: + - step: "middle pitch up" + middle_pitch: + target: *MIDDLE_PITCH_NORMAL - step: "ready to get silver from gripper2" arm: joints: [ *JOINT1_READY_GET_STORED_POS, *JOINT2_GET_STORED_S2_POS, *JOINT3_GET_STORED_SILVER2_POS, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_GET_S2 ] @@ -1443,6 +1474,9 @@ steps_list: gimbal: <<: *FOLLOW_EE LV5_R_S3: + - step: "middle pitch up" + middle_pitch: + target: *MIDDLE_PITCH_NORMAL - step: "lift arm" arm: joints: [ *JOINT1_READY_GET_STORED_POS, 0.113, -2.063, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ] @@ -1487,6 +1521,9 @@ steps_list: gimbal: <<: *FOLLOW_EE LV5_R_G: + - step: "middle pitch up" + middle_pitch: + target: *MIDDLE_PITCH_NORMAL - step: "ready to get stored gold" arm: joints: [ *JOINT1_GET_STORED_GOLD_POS, *JOINT2_GET_STORED_GOLD_POS, *JOINT3_READY_GET_STORED_GOLD_POS, *JOINT4_R90_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ] @@ -1531,6 +1568,9 @@ steps_list: gimbal: <<: *FOLLOW_EE LV4_S1: + - step: "middle pitch up" + middle_pitch: + target: *MIDDLE_PITCH_NORMAL - step: "ready to get silver from gripper1" arm: joints: [ *JOINT1_READY_GET_STORED_POS, *JOINT2_GET_STORED_S1_POS, *JOINT3_GET_STORED_SILVER1_POS, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_GET_S1 ] @@ -1572,6 +1612,9 @@ steps_list: gimbal: <<: *FOLLOW_EE LV4_S2: + - step: "middle pitch up" + middle_pitch: + target: *MIDDLE_PITCH_NORMAL - step: "ready to get silver from gripper2" arm: joints: [ *JOINT1_READY_GET_STORED_POS, *JOINT2_GET_STORED_S2_POS, *JOINT3_GET_STORED_SILVER2_POS, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_GET_S2 ] @@ -1609,6 +1652,9 @@ steps_list: gimbal: <<: *FOLLOW_EE LV4_S3: + - step: "middle pitch up" + middle_pitch: + target: *MIDDLE_PITCH_NORMAL - step: "lift arm" arm: joints: [ *JOINT1_READY_GET_STORED_POS, 0.113, -2.063, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ] @@ -1653,6 +1699,9 @@ steps_list: gimbal: <<: *FOLLOW_EE LV4_G: + - step: "middle pitch up" + middle_pitch: + target: *MIDDLE_PITCH_NORMAL - step: "ready to get stored gold" arm: joints: [ *JOINT1_GET_STORED_GOLD_POS, *JOINT2_GET_STORED_GOLD_POS, *JOINT3_READY_GET_STORED_GOLD_POS, *JOINT4_R90_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ] From c6a3a05b02474345074549bfc2124bde4176f937 Mon Sep 17 00:00:00 2001 From: cc0h <7921166012@qq.com> Date: Thu, 25 Jul 2024 23:05:30 +0800 Subject: [PATCH 58/80] Modify home pos. --- engineer2_arm_config/launch/moveit.rviz | 297 ++++++++++++++++-- .../config/engineer2_steps_list.yaml | 2 +- 2 files changed, 277 insertions(+), 22 deletions(-) diff --git a/engineer2_arm_config/launch/moveit.rviz b/engineer2_arm_config/launch/moveit.rviz index 6a876dd..09ce292 100644 --- a/engineer2_arm_config/launch/moveit.rviz +++ b/engineer2_arm_config/launch/moveit.rviz @@ -5,8 +5,10 @@ Panels: Property Tree Widget: Expanded: - /MotionPlanning1 + - /TF1 + - /TF1/Frames1 Splitter Ratio: 0.5 - Tree Height: 226 + Tree Height: 1301 - Class: rviz/Help Name: Help - Class: rviz/Views @@ -14,6 +16,10 @@ Panels: - /Current View1 Name: Views Splitter Ratio: 0.5 +Preferences: + PromptSaveOnExit: true +Toolbars: + toolButtonStyle: 2 Visualization Manager: Class: "" Displays: @@ -23,7 +29,7 @@ Visualization Manager: Color: 160; 160; 164 Enabled: true Line Style: - Line Width: 0.03 + Line Width: 0.029999999329447746 Value: Lines Name: Grid Normal Cell Count: 0 @@ -35,30 +41,61 @@ Visualization Manager: Plane Cell Count: 10 Reference Frame: Value: true - - Class: moveit_rviz_plugin/MotionPlanning - Enabled: true + - Acceleration_Scaling_Factor: 0.1 + Class: moveit_rviz_plugin/MotionPlanning + Enabled: false + Move Group Namespace: "" + MoveIt_Allow_Approximate_IK: false + MoveIt_Allow_External_Program: false + MoveIt_Allow_Replanning: false + MoveIt_Allow_Sensor_Positioning: false + MoveIt_Planning_Attempts: 10 + MoveIt_Planning_Time: 5 + MoveIt_Use_Cartesian_Path: false + MoveIt_Use_Constraint_Aware_IK: false + MoveIt_Workspace: + Center: + X: 0 + Y: 0 + Z: 0 + Size: + X: 2 + Y: 2 + Z: 2 Name: MotionPlanning Planned Path: - Links: ~ + Color Enabled: false + Interrupt Display: false + Links: + All Links Enabled: true + Expand Joint Details: false + Expand Link Details: false + Expand Tree: false + Link Tree Style: Links in Alphabetic Order Loop Animation: true Robot Alpha: 0.5 + Robot Color: 150; 50; 150 Show Robot Collision: false Show Robot Visual: true Show Trail: false State Display Time: 0.05 s + Trail Step Size: 1 Trajectory Topic: move_group/display_planned_path + Use Sim Time: false Planning Metrics: Payload: 1 Show Joint Torques: false Show Manipulability: false Show Manipulability Index: false Show Weight Limit: false + TextHeight: 0.07999999821186066 Planning Request: Colliding Link Color: 255; 0; 0 Goal State Alpha: 1 Goal State Color: 250; 128; 0 Interactive Marker Size: 0 Joint Violation Color: 255; 0; 255 + Planning Group: engineer_arm Query Goal State: true Query Start State: false Show Workspace: false @@ -68,19 +105,237 @@ Visualization Manager: Robot Description: robot_description Scene Geometry: Scene Alpha: 1 + Scene Color: 50; 230; 50 + Scene Display Time: 0.009999999776482582 Show Scene Geometry: true Voxel Coloring: Z-Axis Voxel Rendering: Occupied Voxels Scene Robot: Attached Body Color: 150; 50; 150 - Links: ~ + Links: + All Links Enabled: true + Expand Joint Details: false + Expand Link Details: false + Expand Tree: false + Link Tree Style: Links in Alphabetic Order Robot Alpha: 0.5 - Show Scene Robot: true + Show Robot Collision: false + Show Robot Visual: true + Value: false + Velocity_Scaling_Factor: 0.1 + - Class: rviz/TF + Enabled: true + Filter (blacklist): "" + Filter (whitelist): "" + Frame Timeout: 15 + Frames: + All Enabled: false + base_link: + Value: false + fake_link4: + Value: false + gold_lifter_link: + Value: false + gold_pusher_link: + Value: false + left_back_wheel: + Value: false + left_front_wheel: + Value: false + link1: + Value: false + link2: + Value: false + link3: + Value: false + link4: + Value: false + link5: + Value: false + link6: + Value: true + map: + Value: false + odom: + Value: false + pitch: + Value: false + right_back_wheel: + Value: false + right_front_wheel: + Value: false + silver_gripper_link: + Value: false + silver_lifter_link: + Value: false + silver_pusher_link: + Value: false + tools_link: + Value: false + yaw: + Value: false + Marker Alpha: 1 + Marker Scale: 1 + Name: TF + Show Arrows: true + Show Axes: true + Show Names: true + Tree: + base_link: + gold_pusher_link: + gold_lifter_link: + {} + left_back_wheel: + {} + left_front_wheel: + {} + link1: + link2: + link3: + fake_link4: + {} + link4: + link5: + link6: + tools_link: + {} + yaw: + pitch: + {} + right_back_wheel: + {} + right_front_wheel: + {} + silver_lifter_link: + silver_pusher_link: + silver_gripper_link: + {} + map: + odom: + {} + Update Interval: 0 + Value: true + - Alpha: 1 + Class: rviz/RobotModel + Collision Enabled: false + Enabled: true + Links: + All Links Enabled: true + Expand Joint Details: false + Expand Link Details: false + Expand Tree: false + Link Tree Style: Links in Alphabetic Order + base_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + fake_link4: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + gold_lifter_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + gold_pusher_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + left_back_wheel: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + left_front_wheel: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + link1: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + link2: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + link3: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + link4: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + link5: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + link6: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + pitch: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + right_back_wheel: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + right_front_wheel: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + silver_gripper_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + silver_lifter_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + silver_pusher_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + tools_link: + Alpha: 1 + Show Axes: false + Show Trail: false + yaw: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + Name: RobotModel + Robot Description: robot_description + TF Prefix: "" + Update Interval: 0 Value: true + Visual Enabled: true Enabled: true Global Options: Background Color: 48; 48; 48 + Default Light: true Fixed Frame: base_link + Frame Rate: 30 Name: root Tools: - Class: rviz/Interact @@ -91,30 +346,30 @@ Visualization Manager: Views: Current: Class: rviz/Orbit - Distance: 2.0 + Distance: 1.3629440069198608 Enable Stereo Rendering: - Stereo Eye Separation: 0.06 + Stereo Eye Separation: 0.05999999865889549 Stereo Focal Distance: 1 Swap Stereo Eyes: false Value: false Field of View: 0.75 Focal Point: - X: -0.1 - Y: 0.25 - Z: 0.30 + X: -0.27402463555336 + Y: -0.39333611726760864 + Z: 0.4662781059741974 Focal Shape Fixed Size: true - Focal Shape Size: 0.05 + Focal Shape Size: 0.05000000074505806 Invert Z Axis: false Name: Current View - Near Clip Distance: 0.01 - Pitch: 0.5 + Near Clip Distance: 0.009999999776482582 + Pitch: 1.419796109199524 Target Frame: base_link - Yaw: -0.6232355833053589 + Yaw: 3.614947557449341 Saved: ~ Window Geometry: Displays: collapsed: false - Height: 848 + Height: 1536 Help: collapsed: false Hide Left Dock: false @@ -123,9 +378,9 @@ Window Geometry: collapsed: false MotionPlanning - Trajectory Slider: collapsed: false - QMainWindow State: 000000ff00000000fd0000000100000000000001f0000002f6fc0200000007fb000000100044006900730070006c006100790073010000003d00000173000000c900fffffffb0000000800480065006c00700000000342000000bb0000006e00fffffffb0000000a00560069006500770073000000010c000000a4000000a400fffffffb0000000c00430061006d00650072006100000002ff000001610000000000000000fb0000001e004d006f00740069006f006e00200050006c0061006e006e0069006e00670100000374000001890000000000000000fb00000044004d006f00740069006f006e0050006c0061006e006e0069006e00670020002d0020005400720061006a006500630074006f0072007900200053006c00690064006500720000000000ffffffff0000001600000016fb0000001c004d006f00740069006f006e0050006c0061006e006e0069006e006701000001b60000017d0000017d00ffffff00000315000002f600000001000000020000000100000002fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + QMainWindow State: 000000ff00000000fd0000000100000000000001f3000005a6fc0200000007fb000000100044006900730070006c006100790073010000003d000005a6000000c900fffffffb0000000800480065006c00700000000342000000bb0000006e00fffffffb0000000a00560069006500770073000000010c000000a4000000a400fffffffb0000000c00430061006d00650072006100000002ff000001610000000000000000fb0000001e004d006f00740069006f006e00200050006c0061006e006e0069006e00670100000374000001890000000000000000fb00000044004d006f00740069006f006e0050006c0061006e006e0069006e00670020002d0020005400720061006a006500630074006f0072007900200053006c00690064006500720000000000ffffffff0000001600000016fb0000001c004d006f00740069006f006e0050006c0061006e006e0069006e006700000002f4000002ef0000017d00ffffff000007bf000005a600000001000000020000000100000002fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 Views: collapsed: false - Width: 1291 - X: 454 - Y: 25 + Width: 2488 + X: 72 + Y: 27 diff --git a/engineer_middleware/config/engineer2_steps_list.yaml b/engineer_middleware/config/engineer2_steps_list.yaml index ae78c86..ddeb557 100644 --- a/engineer_middleware/config/engineer2_steps_list.yaml +++ b/engineer_middleware/config/engineer2_steps_list.yaml @@ -82,7 +82,7 @@ common: joint3: home: &JOINT3_HOME_POS - 0.9 + -2.4 zero: &JOINT3_ZERO_POS 0.00000001 l_exchange: &JOINT3_L_EXCHANGE_POS From d71816f4d2733785984febf94a98140922d46965 Mon Sep 17 00:00:00 2001 From: cc0h <7921166012@qq.com> Date: Fri, 26 Jul 2024 12:12:15 +0800 Subject: [PATCH 59/80] Modify get side big island omtion. --- .../config/engineer2_steps_list.yaml | 27 +++++++++---------- 1 file changed, 13 insertions(+), 14 deletions(-) diff --git a/engineer_middleware/config/engineer2_steps_list.yaml b/engineer_middleware/config/engineer2_steps_list.yaml index ddeb557..a32bafe 100644 --- a/engineer_middleware/config/engineer2_steps_list.yaml +++ b/engineer_middleware/config/engineer2_steps_list.yaml @@ -82,7 +82,7 @@ common: joint3: home: &JOINT3_HOME_POS - -2.4 + -2.3 zero: &JOINT3_ZERO_POS 0.00000001 l_exchange: &JOINT3_L_EXCHANGE_POS @@ -161,7 +161,7 @@ common: middle_pitch: ground_stone: &MIDDLE_PITCH_GROUND_STONE - 1.3 + 1. normal: &MIDDLE_PITCH_NORMAL 0.0 highest: &MIDDLE_PITCH_HIGHEST @@ -1048,7 +1048,7 @@ steps_list: target: *MIDDLE_PITCH_HIGHEST - step: "arm ready" arm: - joints: [ 0.037,0.929,-1.963,-1.474,-1.434,-1.528 ] + joints: [ 0.037,0.9,-1.963,*JOINT4_L90_POS, 0., *JOINT6_R_90 ] common: <<: *NORMALLY tolerance: @@ -1059,21 +1059,21 @@ steps_list: SIDE_BIG_ISLAND0: - step: "arm get gold" arm: - joints: [ 0.037,0.093,-1.38,-1.474,-1.571,-1.528 ] + joints: [ 0.037,0.13,-1.38,*JOINT4_L90_POS,-0., *JOINT6_R_90 ] common: <<: *NORMALLY tolerance: <<: *NORMAL_TOLERANCE - step: "arm lift gold" arm: - joints: [ 0.127,0.093,-1.38,-1.474,-1.571,-1.528 ] + joints: [ 0.127,0.13,-1.38,*JOINT4_L90_POS,-0., *JOINT6_R_90 ] common: <<: *NORMALLY tolerance: <<: *NORMAL_TOLERANCE - step: "arm pull back gold" arm: - joints: [ 0.127,1.04,-1.99,-1.474,-1.321,-1.528 ] + joints: [ 0.127,1.0,-1.99,*JOINT4_L90_POS,-0.36, *JOINT6_R_90 ] common: <<: *NORMALLY tolerance: @@ -1106,7 +1106,7 @@ steps_list: target: *MIDDLE_PITCH_HIGHEST - step: "arm ready" arm: - joints: [ 0.037,0.929,-1.963,-1.474,-1.434,-1.528 ] + joints: [ 0.037,0.9,-1.963,*JOINT4_L90_POS, 0.00001, *JOINT6_R_90 ] common: <<: *QUICKLY tolerance: @@ -1118,20 +1118,19 @@ steps_list: - step: "get gold" gold_pusher: target: -0.443 - delay: 0.3 - - step: "lift gold" - gold_lifter: - target: 0.06 - step: "arm get gold" arm: - joints: [ 0.037,0.093,-1.38,-1.474,-1.571,-1.528 ] + joints: [ 0.037,0.13,-1.38,*JOINT4_L90_POS,-0.00001, *JOINT6_R_90 ] common: <<: *NORMALLY tolerance: <<: *NORMAL_TOLERANCE + - step: "lift gold" + gold_lifter: + target: 0.06 - step: "arm lift gold" arm: - joints: [ 0.127,0.093,-1.38,-1.474,-1.571,-1.528 ] + joints: [ 0.127,0.13,-1.38,*JOINT4_L90_POS,-0.00001, *JOINT6_R_90 ] common: <<: *NORMALLY tolerance: @@ -1147,7 +1146,7 @@ steps_list: <<: *FRONT_POS - step: "arm pull back gold" arm: - joints: [ 0.127,1.04,-1.99,-1.474,-1.321,-1.528 ] + joints: [ 0.127,1.0,-1.99,*JOINT4_L90_POS,-0.36, *JOINT6_R_90 ] common: <<: *NORMALLY tolerance: From 238a3ae943003cfb952fe21f95d2d9acd1fe5264 Mon Sep 17 00:00:00 2001 From: cc0h <7921166012@qq.com> Date: Fri, 26 Jul 2024 23:16:35 +0800 Subject: [PATCH 60/80] Modify motion timeout time. --- .../config/engineer2_steps_list.yaml | 56 +++++++++---------- 1 file changed, 28 insertions(+), 28 deletions(-) diff --git a/engineer_middleware/config/engineer2_steps_list.yaml b/engineer_middleware/config/engineer2_steps_list.yaml index a32bafe..436df58 100644 --- a/engineer_middleware/config/engineer2_steps_list.yaml +++ b/engineer_middleware/config/engineer2_steps_list.yaml @@ -7,11 +7,11 @@ common: normally: &NORMALLY speed: 0.4 accel: 0.3 - timeout: 5 + timeout: 6 quickly: &QUICKLY - speed: 0.6 + speed: 0.8 accel: 0.6 - timeout: 3. + timeout: 6. tolerance: small_tolerance: &SMALL_TOLERANCE @@ -173,7 +173,7 @@ common: common: <<: *NORMALLY tolerance: - <<: *NORMAL_TOLERANCE + <<: *BIGGER_TOLERANCE lv4_exchange: &LV4_EXCHANGE joints: [ *JOINT1_EXCHANGE_POS, *JOINT2_R_EXCHANGE_POS, *JOINT3_R_EXCHANGE_POS, *JOINT4_ZERO_POS, *JOINT5_DOWN_45, *JOINT6_HOME_POS ] @@ -210,7 +210,7 @@ common: frame: base_link position: [ -3., -0.3, 0. ] follow_ee: &FOLLOW_EE - frame: link6 + frame: base_link position: [ 0.15, 0.00, 0. ] chassis_forward: &CHASSIS_FORWARD @@ -342,7 +342,7 @@ steps_list: <<: *LV4_EXCHANGE - step: "gimbal ready" gimbal: - <<: *FOLLOW_EE + <<: *RIGHT_POS LV5_L_EXCHANGE: - step: "middle pitch up" middle_pitch: @@ -352,7 +352,7 @@ steps_list: <<: *LV5_L_EXCHANGE - step: "gimbal ready" gimbal: - <<: *FOLLOW_EE + <<: *RIGHT_POS LV5_R_EXCHANGE: - step: "middle pitch up" @@ -363,7 +363,7 @@ steps_list: <<: *LV5_R_EXCHANGE - step: "gimbal ready" gimbal: - <<: *FOLLOW_EE + <<: *RIGHT_POS GET_SMALL_ISLAND: - step: "middle pitch up" @@ -951,7 +951,7 @@ steps_list: GRIPPER_THREE_SILVER0: - step: "get silver" silver_pusher: - target: -0.03 + target: -0.04 delay: 0.5 - step: "pusher back little" silver_pusher: @@ -1212,7 +1212,7 @@ steps_list: arm: joints: [ *JOINT1_READY_GET_STORED_POS, *JOINT2_GET_STORED_S1_POS, *JOINT3_GET_STORED_SILVER1_POS, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_GET_S1 ] common: - <<: *NORMALLY + <<: *QUICKLY tolerance: <<: *NORMAL_TOLERANCE @@ -1247,7 +1247,7 @@ steps_list: <<: *LV5_L_EXCHANGE - step: "gimbal ready" gimbal: - <<: *FOLLOW_EE + <<: *RIGHT_POS @@ -1259,7 +1259,7 @@ steps_list: arm: joints: [ *JOINT1_READY_GET_STORED_POS, *JOINT2_GET_STORED_S2_POS, *JOINT3_GET_STORED_SILVER2_POS, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_GET_S2 ] common: - <<: *NORMALLY + <<: *QUICKLY tolerance: <<: *NORMAL_TOLERANCE - step: "open main gripper" @@ -1290,7 +1290,7 @@ steps_list: <<: *LV5_L_EXCHANGE - step: "gimbal ready" gimbal: - <<: *FOLLOW_EE + <<: *RIGHT_POS @@ -1302,7 +1302,7 @@ steps_list: arm: joints: [ *JOINT1_READY_GET_STORED_POS, 0.113, -2.063, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ] common: - <<: *NORMALLY + <<: *QUICKLY tolerance: <<: *NORMAL_TOLERANCE - step: "ready to get silver from gripper3" @@ -1340,7 +1340,7 @@ steps_list: <<: *LV5_L_EXCHANGE - step: "gimbal ready" gimbal: - <<: *FOLLOW_EE + <<: *RIGHT_POS LV5_L_G: - step: "middle pitch up" middle_pitch: @@ -1349,7 +1349,7 @@ steps_list: arm: joints: [ *JOINT1_GET_STORED_GOLD_POS, *JOINT2_GET_STORED_GOLD_POS, *JOINT3_READY_GET_STORED_GOLD_POS, *JOINT4_R90_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ] common: - <<: *NORMALLY + <<: *QUICKLY tolerance: <<: *NORMAL_TOLERANCE - step: "open main gripper" @@ -1387,7 +1387,7 @@ steps_list: <<: *LV5_L_EXCHANGE - step: "gimbal ready" gimbal: - <<: *FOLLOW_EE + <<: *RIGHT_POS LV5_R_S1: - step: "middle pitch up" middle_pitch: @@ -1396,7 +1396,7 @@ steps_list: arm: joints: [ *JOINT1_READY_GET_STORED_POS, *JOINT2_GET_STORED_S1_POS, *JOINT3_GET_STORED_SILVER1_POS, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_GET_S1 ] common: - <<: *NORMALLY + <<: *QUICKLY tolerance: <<: *NORMAL_TOLERANCE @@ -1431,7 +1431,7 @@ steps_list: <<: *LV5_R_EXCHANGE - step: "gimbal ready" gimbal: - <<: *FOLLOW_EE + <<: *RIGHT_POS LV5_R_S2: - step: "middle pitch up" middle_pitch: @@ -1440,7 +1440,7 @@ steps_list: arm: joints: [ *JOINT1_READY_GET_STORED_POS, *JOINT2_GET_STORED_S2_POS, *JOINT3_GET_STORED_SILVER2_POS, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_GET_S2 ] common: - <<: *NORMALLY + <<: *QUICKLY tolerance: <<: *NORMAL_TOLERANCE - step: "open main gripper" @@ -1471,7 +1471,7 @@ steps_list: <<: *LV5_R_EXCHANGE - step: "gimbal ready" gimbal: - <<: *FOLLOW_EE + <<: *RIGHT_POS LV5_R_S3: - step: "middle pitch up" middle_pitch: @@ -1480,7 +1480,7 @@ steps_list: arm: joints: [ *JOINT1_READY_GET_STORED_POS, 0.113, -2.063, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ] common: - <<: *NORMALLY + <<: *QUICKLY tolerance: <<: *NORMAL_TOLERANCE - step: "ready to get silver from gripper3" @@ -1518,7 +1518,7 @@ steps_list: <<: *LV5_R_EXCHANGE - step: "gimbal ready" gimbal: - <<: *FOLLOW_EE + <<: *RIGHT_POS LV5_R_G: - step: "middle pitch up" middle_pitch: @@ -1565,7 +1565,7 @@ steps_list: <<: *LV5_R_EXCHANGE - step: "gimbal ready" gimbal: - <<: *FOLLOW_EE + <<: *RIGHT_POS LV4_S1: - step: "middle pitch up" middle_pitch: @@ -1609,7 +1609,7 @@ steps_list: <<: *LV4_EXCHANGE - step: "gimbal ready" gimbal: - <<: *FOLLOW_EE + <<: *RIGHT_POS LV4_S2: - step: "middle pitch up" middle_pitch: @@ -1649,7 +1649,7 @@ steps_list: <<: *LV4_EXCHANGE - step: "gimbal ready" gimbal: - <<: *FOLLOW_EE + <<: *RIGHT_POS LV4_S3: - step: "middle pitch up" middle_pitch: @@ -1696,7 +1696,7 @@ steps_list: <<: *LV4_EXCHANGE - step: "gimbal ready" gimbal: - <<: *FOLLOW_EE + <<: *RIGHT_POS LV4_G: - step: "middle pitch up" middle_pitch: @@ -1743,4 +1743,4 @@ steps_list: <<: *LV4_EXCHANGE - step: "gimbal ready" gimbal: - <<: *FOLLOW_EE + <<: *RIGHT_POS From c6da30d619ae50156045776b35fc91d1d22b7fe5 Mon Sep 17 00:00:00 2001 From: cc0h <7921166012@qq.com> Date: Sat, 27 Jul 2024 03:06:39 +0800 Subject: [PATCH 61/80] Add get ground stone and fix get side big island. --- .../config/engineer2_steps_list.yaml | 51 ++++++++++++------- 1 file changed, 33 insertions(+), 18 deletions(-) diff --git a/engineer_middleware/config/engineer2_steps_list.yaml b/engineer_middleware/config/engineer2_steps_list.yaml index 436df58..607bd3f 100644 --- a/engineer_middleware/config/engineer2_steps_list.yaml +++ b/engineer_middleware/config/engineer2_steps_list.yaml @@ -5,11 +5,11 @@ common: accel: 0.1 timeout: 15. normally: &NORMALLY - speed: 0.4 + speed: 0.6 accel: 0.3 timeout: 6 quickly: &QUICKLY - speed: 0.8 + speed: 1.0 accel: 0.6 timeout: 6. @@ -26,10 +26,8 @@ common: joint1: home: &JOINT1_HOME_POS 0.1 - ready_get_ground: &JOINT1_READY_GET_GROUND_POS + ready_get_ground: &JOINT1_GET_GROUND_POS 0.005 - get_ground: &JOINT1_GET_GROUND_POS - 0.4 exchange: &JOINT1_EXCHANGE_POS 0.35 ready_get_small: &JOINT1_READY_GET_SMALL_POS @@ -536,7 +534,7 @@ steps_list: GROUND_STONE: - step: "arm ready get ground stone" arm: - joints: [*JOINT1_READY_GET_GROUND_POS, *JOINT2_HOME_POS, *JOINT3_ZERO_POS, *JOINT4_HOME_POS, *JOINT5_HOME_POS, *JOINT6_HOME_POS] + joints: [*JOINT1_GET_GROUND_POS, 0.94, -1.266, *JOINT4_ZERO_POS, 1.112 , -0.655] common: <<: *NORMALLY tolerance: @@ -547,7 +545,7 @@ steps_list: GROUND_STONE0: - step: "move arm to get ground stone" arm: - joints: [ *JOINT1_READY_GET_GROUND_POS, *JOINT2_HOME_POS, *JOINT3_ZERO_POS, *JOINT4_HOME_POS, *JOINT5_HOME_POS, *JOINT6_HOME_POS ] + joints: [*JOINT1_GET_GROUND_POS, 0.94, -2.066, *JOINT4_ZERO_POS, 1.112 , -0.655] common: <<: *NORMALLY tolerance: @@ -555,12 +553,12 @@ steps_list: - step: "open main gripper" gripper: <<: *OPEN_MAIN_GRIPPER - - step: "ready to get ground stone" + - step: "get ground stone" middle_pitch: - target: *MIDDLE_PITCH_GROUND_STONE + target: 0.49 - step: "arm pull up stone" arm: - joints: [*JOINT1_READY_GET_GROUND_POS, *JOINT2_EXCHANGE_POS, *JOINT3_ZERO_POS, *JOINT4_ZERO_POS, *JOINT5_HOME_POS, *JOINT6_HOME_POS] + joints: [0.2, 0.94, -2.066, *JOINT4_ZERO_POS, 1.112 , -0.655] common: <<: *NORMALLY tolerance: @@ -1048,7 +1046,7 @@ steps_list: target: *MIDDLE_PITCH_HIGHEST - step: "arm ready" arm: - joints: [ 0.037,0.9,-1.963,*JOINT4_L90_POS, 0., *JOINT6_R_90 ] + joints: [ 0.037,0.9,-1.963,*JOINT4_L90_POS, 0.00001, *JOINT6_R_90 ] common: <<: *NORMALLY tolerance: @@ -1059,18 +1057,21 @@ steps_list: SIDE_BIG_ISLAND0: - step: "arm get gold" arm: - joints: [ 0.037,0.13,-1.38,*JOINT4_L90_POS,-0., *JOINT6_R_90 ] + joints: [ 0.037,0.12,-1.47,*JOINT4_L90_POS,-0.00001, *JOINT6_R_90 ] common: <<: *NORMALLY tolerance: <<: *NORMAL_TOLERANCE - step: "arm lift gold" arm: - joints: [ 0.127,0.13,-1.38,*JOINT4_L90_POS,-0., *JOINT6_R_90 ] + joints: [ 0.127,0.12,-1.47,*JOINT4_L90_POS,-0.00001, *JOINT6_R_90 ] common: <<: *NORMALLY tolerance: <<: *NORMAL_TOLERANCE + - step: "gimbal look front" + gimbal: + <<: *FRONT_POS - step: "arm pull back gold" arm: joints: [ 0.127,1.0,-1.99,*JOINT4_L90_POS,-0.36, *JOINT6_R_90 ] @@ -1078,9 +1079,16 @@ steps_list: <<: *NORMALLY tolerance: <<: *NORMAL_TOLERANCE - - step: "gimbal look front" - gimbal: - <<: *FRONT_POS + - step: "chassis left" + chassis: + <<: *CHASSIS_MOVE_LEFT + - step: "move stone towards down" + arm: + joints: [ 0.127, 1.0, -2.388, 0.0001, 1.57, -1.57 ] + common: + <<: *NORMALLY + tolerance: + <<: *NORMAL_TOLERANCE BOTH_BIG_ISLAND: - step: "middle pitch up" @@ -1120,7 +1128,7 @@ steps_list: target: -0.443 - step: "arm get gold" arm: - joints: [ 0.037,0.13,-1.38,*JOINT4_L90_POS,-0.00001, *JOINT6_R_90 ] + joints: [ 0.037,0.12,-1.47,*JOINT4_L90_POS,-0.00001, *JOINT6_R_90 ] common: <<: *NORMALLY tolerance: @@ -1130,7 +1138,7 @@ steps_list: target: 0.06 - step: "arm lift gold" arm: - joints: [ 0.127,0.13,-1.38,*JOINT4_L90_POS,-0.00001, *JOINT6_R_90 ] + joints: [ 0.127,0.12,-1.47,*JOINT4_L90_POS,-0.00001, *JOINT6_R_90 ] common: <<: *NORMALLY tolerance: @@ -1154,6 +1162,13 @@ steps_list: - step: "chassis left" chassis: <<: *CHASSIS_MOVE_LEFT + - step: "move stone towards down" + arm: + joints: [ 0.127, 1.0, -2.388, 0.0001, 1.57, -1.57 ] + common: + <<: *NORMALLY + tolerance: + <<: *NORMAL_TOLERANCE OM: - step: "main open" From c7a05eb0a4ed1cd02164034f4707e6aa06ae29d0 Mon Sep 17 00:00:00 2001 From: cc0h <7921166012@qq.com> Date: Mon, 29 Jul 2024 19:08:51 +0800 Subject: [PATCH 62/80] Add default 0.05s delay for gpio motion. --- engineer_middleware/include/engineer_middleware/motion.h | 7 +++++++ engineer_middleware/include/engineer_middleware/step.h | 2 ++ 2 files changed, 9 insertions(+) diff --git a/engineer_middleware/include/engineer_middleware/motion.h b/engineer_middleware/include/engineer_middleware/motion.h index 0827a84..2846822 100644 --- a/engineer_middleware/include/engineer_middleware/motion.h +++ b/engineer_middleware/include/engineer_middleware/motion.h @@ -568,7 +568,14 @@ class GpioMotion : public PublishMotion return PublishMotion::move(); } + bool isFinish() override + { + return ((ros::Time::now() - start_time_).toSec() > delay_); + } + private: + ros::Time start_time_; + double delay_{ 0.05 }; bool state_; int pin_; }; diff --git a/engineer_middleware/include/engineer_middleware/step.h b/engineer_middleware/include/engineer_middleware/step.h index 7239b13..563c4e4 100644 --- a/engineer_middleware/include/engineer_middleware/step.h +++ b/engineer_middleware/include/engineer_middleware/step.h @@ -216,6 +216,8 @@ class Step success &= silver_pusher_motion_->isFinish(); if (silver_rotator_motion_) success &= silver_rotator_motion_->isFinish(); + if (gpio_motion_) + success &= gpio_motion_->isFinish(); if (gold_pusher_motion_) success &= gold_pusher_motion_->isFinish(); if (gold_lifter_motion_) From 490045f7731b635ec4a9d384f82ccf5eb9a9257f Mon Sep 17 00:00:00 2001 From: cc0h <7921166012@qq.com> Date: Mon, 29 Jul 2024 20:50:04 +0800 Subject: [PATCH 63/80] Set higer speed and accel limit. --- engineer2_arm_config/config/joint_limits.yaml | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/engineer2_arm_config/config/joint_limits.yaml b/engineer2_arm_config/config/joint_limits.yaml index 6699247..72eee70 100644 --- a/engineer2_arm_config/config/joint_limits.yaml +++ b/engineer2_arm_config/config/joint_limits.yaml @@ -2,8 +2,8 @@ # For beginners, we downscale velocity and acceleration limits. # You can always specify higher scaling factors (<= 1.0) in your motion requests. # Increase the values below to 1.0 to always move at maximum speed. -default_velocity_scaling_factor: 0.1 -default_acceleration_scaling_factor: 0.1 +default_velocity_scaling_factor: 1.0 +default_acceleration_scaling_factor: 1.0 # Specific joint properties can be changed with the keys [max_position, min_position, max_velocity, max_acceleration] # Joint limits can be turned off with [has_velocity_limits, has_acceleration_limits] From 3414f33439245fb6656f6a063e1eae2db6def0bf Mon Sep 17 00:00:00 2001 From: cc0h <7921166012@qq.com> Date: Mon, 29 Jul 2024 20:50:36 +0800 Subject: [PATCH 64/80] Record. --- .../config/engineer2_steps_list.yaml | 177 +++++++++++++++--- 1 file changed, 146 insertions(+), 31 deletions(-) diff --git a/engineer_middleware/config/engineer2_steps_list.yaml b/engineer_middleware/config/engineer2_steps_list.yaml index 607bd3f..b851f0c 100644 --- a/engineer_middleware/config/engineer2_steps_list.yaml +++ b/engineer_middleware/config/engineer2_steps_list.yaml @@ -1,16 +1,16 @@ common: speed: slowly: &SLOWLY - speed: 0.15 + speed: 0.4 accel: 0.1 timeout: 15. normally: &NORMALLY - speed: 0.6 - accel: 0.3 + speed: 2.5 + accel: 1.5 timeout: 6 quickly: &QUICKLY - speed: 1.0 - accel: 0.6 + speed: 3 + accel: 2 timeout: 6. tolerance: @@ -19,7 +19,7 @@ common: normal_tolerance: &NORMAL_TOLERANCE tolerance_joints: [ 0.012, 0.02, 0.015, 0.1, 0.2, 0.15, 0.07 ] bigger_tolerance: &BIGGER_TOLERANCE - tolerance_joints: [ 0.015, 0.015, 0.03, 0.35, 0.2, 0.2, 0.1 ] + tolerance_joints: [ 0.015, 0.04, 0.03, 0.35, 0.2, 0.2, 0.1 ] max_tolerance: &MAX_TOLERANCE tolerance_joints: [ 0.03, 0.03, 0.03, 0.3, 0.3, 0.3 , 0.3] @@ -54,9 +54,9 @@ common: exchange: &JOINT2_EXCHANGE_POS -1.57 l_exchange: &JOINT2_L_EXCHANGE_POS - -0.984 + -0.3 r_exchange: &JOINT2_R_EXCHANGE_POS - -2.156 + -2.5 get_small_island_l: &JOINT2_GET_SMALL_ISLAND_L_POS -0.098 get_small_island_m: &JOINT2_GET_SMALL_ISLAND_M_POS @@ -84,9 +84,9 @@ common: zero: &JOINT3_ZERO_POS 0.00000001 l_exchange: &JOINT3_L_EXCHANGE_POS - -1.165 + -0.7 r_exchange: &JOINT3_R_EXCHANGE_POS - 1.165 + 0.7 get_small_island_l: &JOINT3_GET_SMALL_ISLAND_L_POS -1.948 get_small_island_m: &JOINT3_GET_SMALL_ISLAND_M_POS @@ -180,14 +180,14 @@ common: tolerance: <<: *NORMAL_TOLERANCE - lv5_l_exchange: &LV5_L_EXCHANGE + lv5_r_exchange: &LV5_L_EXCHANGE joints: [ *JOINT1_EXCHANGE_POS, *JOINT2_L_EXCHANGE_POS, *JOINT3_L_EXCHANGE_POS, *JOINT4_R90_POS, *JOINT5_DOWN_90, *JOINT6_L_90] common: <<: *NORMALLY tolerance: <<: *NORMAL_TOLERANCE - lv5_r_exchange: &LV5_R_EXCHANGE + lv5_l_exchange: &LV5_R_EXCHANGE joints: [ *JOINT1_EXCHANGE_POS, *JOINT2_R_EXCHANGE_POS, *JOINT3_R_EXCHANGE_POS, *JOINT4_L90_POS, *JOINT5_DOWN_90, *JOINT6_R_90] common: <<: *NORMALLY @@ -220,14 +220,23 @@ common: common: timeout: 2. + chassis_turn_180: &CHASSIS_TURN_180 + frame: base_link + position: [ 0., 0. ] + yaw: 1.57 + chassis_tolerance_position: 0.1 + chassis_tolerance_angular: 0.3 + common: + timeout: 4. + chassis_fmove_left: &CHASSIS_MOVE_LEFT frame: base_link - position: [ 0.0, 0.2 ] + position: [ 0.0, 0.4 ] yaw: 0.0 chassis_tolerance_position: 0.1 chassis_tolerance_angular: 0.2 common: - timeout: 2. + timeout: 3. gripper: @@ -295,6 +304,9 @@ steps_list: middle_pitch: target: *MIDDLE_PITCH_NORMAL HOME: + - step: "main close" + gripper: + <<: *CLOSE_MAIN_GRIPPER - step: "pusher back" silver_pusher: target: 0. @@ -313,6 +325,9 @@ steps_list: - step: "gimbal look front" gimbal: <<: *FRONT_POS + - step: "main close" + gripper: + <<: *CLOSE_MAIN_GRIPPER - step: "arm home pos" arm: <<: *HOME @@ -320,13 +335,63 @@ steps_list: middle_pitch: target: *MIDDLE_PITCH_GROUND_STONE + CALIBRATION: + - step: "middle pitch up" + middle_pitch: + target: *MIDDLE_PITCH_NORMAL + - step: "main close" + gripper: + <<: *CLOSE_MAIN_GRIPPER + - step: "s1 close" + gripper: + <<: *CLOSE_S1 + - step: "s2 close" + gripper: + <<: *CLOSE_S2 + - step: "s3 close" + gripper: + <<: *CLOSE_S3 + - step: "g1 close" + gripper: + <<: *CLOSE_GOLD + - step: "close silver gripper" + gripper: + <<: *CLOSE_S_GRIPPER + - step: "gimbal look front" + gimbal: + <<: *FRONT_POS + - step: "main close" + gripper: + <<: *CLOSE_MAIN_GRIPPER + HOME_WITH_PITCH: + - step: "main close" + gripper: + <<: *CLOSE_MAIN_GRIPPER + - step: "s1 close" + gripper: + <<: *CLOSE_S1 + - step: "s2 close" + gripper: + <<: *CLOSE_S2 + - step: "s3 close" + gripper: + <<: *CLOSE_S3 + - step: "g1 close" + gripper: + <<: *CLOSE_GOLD + - step: "close silver gripper" + gripper: + <<: *CLOSE_S_GRIPPER - step: "gimbal look front" gimbal: <<: *FRONT_POS - step: "arm home pos" arm: <<: *HOME + - step: "main close" + gripper: + <<: *CLOSE_MAIN_GRIPPER - step: "middle pitch down" middle_pitch: target: *MIDDLE_PITCH_GROUND_STONE @@ -532,9 +597,12 @@ steps_list: <<: *NORMAL_TOLERANCE GROUND_STONE: + - step: "middle pitch up" + middle_pitch: + target: *MIDDLE_PITCH_NORMAL - step: "arm ready get ground stone" arm: - joints: [*JOINT1_GET_GROUND_POS, 0.94, -1.266, *JOINT4_ZERO_POS, 1.112 , -0.655] + joints: [*JOINT1_GET_GROUND_POS, 0.94, -1.266, *JOINT4_ZERO_POS, 1.112 , -0.397] common: <<: *NORMALLY tolerance: @@ -545,7 +613,7 @@ steps_list: GROUND_STONE0: - step: "move arm to get ground stone" arm: - joints: [*JOINT1_GET_GROUND_POS, 0.94, -2.066, *JOINT4_ZERO_POS, 1.112 , -0.655] + joints: [*JOINT1_GET_GROUND_POS, 0.943, -1.863, *JOINT4_ZERO_POS, 1.112 , -0.397] common: <<: *NORMALLY tolerance: @@ -556,9 +624,10 @@ steps_list: - step: "get ground stone" middle_pitch: target: 0.49 + delay: 0.5 - step: "arm pull up stone" arm: - joints: [0.2, 0.94, -2.066, *JOINT4_ZERO_POS, 1.112 , -0.655] + joints: [0.2, 0.943, -1.863, *JOINT4_ZERO_POS, 1.112 , -0.397] common: <<: *NORMALLY tolerance: @@ -931,6 +1000,9 @@ steps_list: - step: "pusher back" silver_pusher: target: -0.00001 + - step: "turn 180" + chassis: + <<: *CHASSIS_TURN_180 - step: "open gripper" gripper: <<: *OPEN_S1 @@ -1046,9 +1118,9 @@ steps_list: target: *MIDDLE_PITCH_HIGHEST - step: "arm ready" arm: - joints: [ 0.037,0.9,-1.963,*JOINT4_L90_POS, 0.00001, *JOINT6_R_90 ] + joints: [ 0.040,0.885,-2.107,*JOINT4_L90_POS,-0.00001, *JOINT6_R_90 ] common: - <<: *NORMALLY + <<: *QUICKLY tolerance: <<: *NORMAL_TOLERANCE - step: "open main gripper" @@ -1057,14 +1129,14 @@ steps_list: SIDE_BIG_ISLAND0: - step: "arm get gold" arm: - joints: [ 0.037,0.12,-1.47,*JOINT4_L90_POS,-0.00001, *JOINT6_R_90 ] + joints: [ 0.040,0.138,-1.649,*JOINT4_L90_POS,-0.00001, *JOINT6_R_90 ] common: <<: *NORMALLY tolerance: <<: *NORMAL_TOLERANCE - step: "arm lift gold" arm: - joints: [ 0.127,0.12,-1.47,*JOINT4_L90_POS,-0.00001, *JOINT6_R_90 ] + joints: [ 0.115,0.138,-1.649,*JOINT4_L90_POS,-0.00001, *JOINT6_R_90 ] common: <<: *NORMALLY tolerance: @@ -1074,7 +1146,7 @@ steps_list: <<: *FRONT_POS - step: "arm pull back gold" arm: - joints: [ 0.127,1.0,-1.99,*JOINT4_L90_POS,-0.36, *JOINT6_R_90 ] + joints: [ 0.115,0.991,-2.142,*JOINT4_L90_POS,-0.335, *JOINT6_R_90 ] common: <<: *NORMALLY tolerance: @@ -1084,7 +1156,7 @@ steps_list: <<: *CHASSIS_MOVE_LEFT - step: "move stone towards down" arm: - joints: [ 0.127, 1.0, -2.388, 0.0001, 1.57, -1.57 ] + joints: [ 0.120, 0.998, -2.388, 0.0001, 1.57, -1.57 ] common: <<: *NORMALLY tolerance: @@ -1097,6 +1169,9 @@ steps_list: - step: "gimbal look right" gimbal: <<: *RIGHT_POS + - step: "open main gripper" + gripper: + <<: *OPEN_MAIN_GRIPPER - step: "gimbal look right" gimbal: <<: *RIGHT_POS @@ -1114,7 +1189,7 @@ steps_list: target: *MIDDLE_PITCH_HIGHEST - step: "arm ready" arm: - joints: [ 0.037,0.9,-1.963,*JOINT4_L90_POS, 0.00001, *JOINT6_R_90 ] + joints: [ 0.040,0.885,-2.107,*JOINT4_L90_POS,-0.00001, *JOINT6_R_90 ] common: <<: *QUICKLY tolerance: @@ -1128,17 +1203,17 @@ steps_list: target: -0.443 - step: "arm get gold" arm: - joints: [ 0.037,0.12,-1.47,*JOINT4_L90_POS,-0.00001, *JOINT6_R_90 ] + joints: [ 0.040,0.138,-1.649,*JOINT4_L90_POS,-0.00001, *JOINT6_R_90 ] common: <<: *NORMALLY tolerance: - <<: *NORMAL_TOLERANCE + <<: *BIGGER_TOLERANCE - step: "lift gold" gold_lifter: - target: 0.06 + target: 0.055 - step: "arm lift gold" arm: - joints: [ 0.127,0.12,-1.47,*JOINT4_L90_POS,-0.00001, *JOINT6_R_90 ] + joints: [ 0.115,0.138,-1.649,*JOINT4_L90_POS,-0.00001, *JOINT6_R_90 ] common: <<: *NORMALLY tolerance: @@ -1154,17 +1229,17 @@ steps_list: <<: *FRONT_POS - step: "arm pull back gold" arm: - joints: [ 0.127,1.0,-1.99,*JOINT4_L90_POS,-0.36, *JOINT6_R_90 ] + joints: [ 0.115,0.991,-2.142,*JOINT4_L90_POS,-0.335, *JOINT6_R_90 ] common: <<: *NORMALLY tolerance: - <<: *NORMAL_TOLERANCE + <<: *BIGGER_TOLERANCE - step: "chassis left" chassis: <<: *CHASSIS_MOVE_LEFT - step: "move stone towards down" arm: - joints: [ 0.127, 1.0, -2.388, 0.0001, 1.57, -1.57 ] + joints: [ 0.127, 0.997, -1.163, 0.0001, 1.57, -1.57 ] common: <<: *NORMALLY tolerance: @@ -1219,6 +1294,46 @@ steps_list: gripper: <<: *CLOSE_S_GRIPPER + CA: + - step: "main close" + gripper: + <<: *CLOSE_MAIN_GRIPPER + - step: "s1 close" + gripper: + <<: *CLOSE_S1 + - step: "s2 close" + gripper: + <<: *CLOSE_S2 + - step: "s3 close" + gripper: + <<: *CLOSE_S3 + - step: "g1 close" + gripper: + <<: *CLOSE_GOLD + - step: "close silver gripper" + gripper: + <<: *CLOSE_S_GRIPPER + + OA: + - step: "main OPEN" + gripper: + <<: *OPEN_MAIN_GRIPPER + - step: "s1 OPEN" + gripper: + <<: *OPEN_S1 + - step: "s2 OPEN" + gripper: + <<: *OPEN_S2 + - step: "s3 OPEN" + gripper: + <<: *OPEN_S3 + - step: "g1 OPEN" + gripper: + <<: *OPEN_GOLD + - step: "OPEN silver gripper" + gripper: + <<: *OPEN_S_GRIPPER + LV5_L_S1: - step: "middle pitch up" middle_pitch: From 7a22e9afd7e2bc50878f774aedacc7d90d7089aa Mon Sep 17 00:00:00 2001 From: cc0h <7921166012@qq.com> Date: Mon, 29 Jul 2024 21:23:05 +0800 Subject: [PATCH 65/80] Add gpio motion default delay to 0.1s. --- engineer_middleware/include/engineer_middleware/motion.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/engineer_middleware/include/engineer_middleware/motion.h b/engineer_middleware/include/engineer_middleware/motion.h index 2846822..91057b1 100644 --- a/engineer_middleware/include/engineer_middleware/motion.h +++ b/engineer_middleware/include/engineer_middleware/motion.h @@ -575,7 +575,7 @@ class GpioMotion : public PublishMotion private: ros::Time start_time_; - double delay_{ 0.05 }; + double delay_{ 0.1 }; bool state_; int pin_; }; From a4f1b5ba00864f64d4374dc2fadcb7ef22d1a72a Mon Sep 17 00:00:00 2001 From: cc0h <7921166012@qq.com> Date: Tue, 30 Jul 2024 05:25:05 +0800 Subject: [PATCH 66/80] Modify joint3 speed limit in moveit config file. --- engineer2_arm_config/config/joint_limits.yaml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/engineer2_arm_config/config/joint_limits.yaml b/engineer2_arm_config/config/joint_limits.yaml index 72eee70..75658e6 100644 --- a/engineer2_arm_config/config/joint_limits.yaml +++ b/engineer2_arm_config/config/joint_limits.yaml @@ -20,7 +20,7 @@ joint_limits: max_acceleration: 0 joint3: has_velocity_limits: true - max_velocity: 3.76 + max_velocity: 5.44 has_acceleration_limits: false max_acceleration: 0 joint4: From 2db342b3f3887d65cec0f20614fe9386971b0336 Mon Sep 17 00:00:00 2001 From: cc0h <7921166012@qq.com> Date: Tue, 30 Jul 2024 05:26:29 +0800 Subject: [PATCH 67/80] Fix unavailable gpio motion delay. --- engineer_middleware/include/engineer_middleware/motion.h | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/engineer_middleware/include/engineer_middleware/motion.h b/engineer_middleware/include/engineer_middleware/motion.h index 91057b1..b2bdf94 100644 --- a/engineer_middleware/include/engineer_middleware/motion.h +++ b/engineer_middleware/include/engineer_middleware/motion.h @@ -528,6 +528,7 @@ class GpioMotion : public PublishMotion GpioMotion(XmlRpc::XmlRpcValue& motion, ros::Publisher& interface) : PublishMotion(motion, interface) { + delay_ = xmlRpcGetDouble(motion, "delay", 0.01); msg_.gpio_state.assign(8, false); msg_.gpio_name.assign(8, "no_registered"); pin_ = motion["pin"]; @@ -564,6 +565,7 @@ class GpioMotion : public PublishMotion } bool move() override { + start_time_ = ros::Time::now(); msg_.gpio_state[pin_] = state_; return PublishMotion::move(); } @@ -575,7 +577,7 @@ class GpioMotion : public PublishMotion private: ros::Time start_time_; - double delay_{ 0.1 }; + double delay_; bool state_; int pin_; }; From 97e20a9f19ff06a7c9630fb6735488637ddad3e6 Mon Sep 17 00:00:00 2001 From: cc0h <7921166012@qq.com> Date: Tue, 30 Jul 2024 06:39:31 +0800 Subject: [PATCH 68/80] Adjust side gold stone motion, add motion to exchange side gold at front of chassis. --- .../config/engineer2_steps_list.yaml | 56 +++++++++++++++---- 1 file changed, 45 insertions(+), 11 deletions(-) diff --git a/engineer_middleware/config/engineer2_steps_list.yaml b/engineer_middleware/config/engineer2_steps_list.yaml index b851f0c..fc615d9 100644 --- a/engineer_middleware/config/engineer2_steps_list.yaml +++ b/engineer_middleware/config/engineer2_steps_list.yaml @@ -167,7 +167,7 @@ common: arm: home: &HOME - joints: [ *JOINT1_HOME_POS, *JOINT2_HOME_POS, *JOINT3_HOME_POS, *JOINT4_HOME_POS, *JOINT5_HOME_POS, *JOINT6_HOME_POS ] + joints: [ *JOINT1_HOME_POS, *JOINT2_HOME_POS, *JOINT3_HOME_POS, *JOINT4_ZERO_POS, *JOINT5_HOME_POS, *JOINT6_HOME_POS ] common: <<: *NORMALLY tolerance: @@ -428,6 +428,38 @@ steps_list: gimbal: <<: *RIGHT_POS + LV5_R_CAR_FRONT_EXCHANGE: + - step: "middle pitch up" + middle_pitch: + target: *MIDDLE_PITCH_NORMAL + delay: 0.3 + - step: "arm enter exchange pos" + arm: + joints: [ *JOINT1_EXCHANGE_POS, -0.774, *JOINT3_R_EXCHANGE_POS, *JOINT4_L90_POS, *JOINT5_DOWN_90, *JOINT6_R_90 ] + common: + <<: *NORMALLY + tolerance: + <<: *NORMAL_TOLERANCE + - step: "gimbal ready" + gimbal: + <<: *FRONT_POS + + LV5_L_CAR_FRONT_EXCHANGE: + - step: "middle pitch up" + middle_pitch: + target: *MIDDLE_PITCH_NORMAL + delay: 0.3 + - step: "arm enter exchange pos" + arm: + joints: [ *JOINT1_EXCHANGE_POS, 1.001, *JOINT3_L_EXCHANGE_POS, *JOINT4_R90_POS, *JOINT5_DOWN_90, *JOINT6_L_90 ] + common: + <<: *NORMALLY + tolerance: + <<: *NORMAL_TOLERANCE + - step: "gimbal ready" + gimbal: + <<: *FRONT_POS + GET_SMALL_ISLAND: - step: "middle pitch up" middle_pitch: @@ -1116,9 +1148,10 @@ steps_list: - step: "middle pitch up" middle_pitch: target: *MIDDLE_PITCH_HIGHEST + delay: 0.3 - step: "arm ready" arm: - joints: [ 0.040,0.885,-2.107,*JOINT4_L90_POS,-0.00001, *JOINT6_R_90 ] + joints: [ 0.040,0.885,-2.107,*JOINT4_R90_POS,-0.00001, *JOINT6_HOME_POS ] common: <<: *QUICKLY tolerance: @@ -1129,14 +1162,14 @@ steps_list: SIDE_BIG_ISLAND0: - step: "arm get gold" arm: - joints: [ 0.040,0.138,-1.649,*JOINT4_L90_POS,-0.00001, *JOINT6_R_90 ] + joints: [ 0.040,0.162,-1.414,*JOINT4_R90_POS, 0.097, *JOINT6_HOME_POS ] common: <<: *NORMALLY tolerance: <<: *NORMAL_TOLERANCE - step: "arm lift gold" arm: - joints: [ 0.115,0.138,-1.649,*JOINT4_L90_POS,-0.00001, *JOINT6_R_90 ] + joints: [ 0.115,0.162,-1.414,*JOINT4_R90_POS,0.097, *JOINT6_HOME_POS ] common: <<: *NORMALLY tolerance: @@ -1146,17 +1179,17 @@ steps_list: <<: *FRONT_POS - step: "arm pull back gold" arm: - joints: [ 0.115,0.991,-2.142,*JOINT4_L90_POS,-0.335, *JOINT6_R_90 ] + joints: [ 0.115,0.988,-2.023,*JOINT4_R90_POS,0.233, *JOINT6_HOME_POS ] common: <<: *NORMALLY tolerance: - <<: *NORMAL_TOLERANCE + <<: *BIGGER_TOLERANCE - step: "chassis left" chassis: <<: *CHASSIS_MOVE_LEFT - step: "move stone towards down" arm: - joints: [ 0.120, 0.998, -2.388, 0.0001, 1.57, -1.57 ] + joints: [ 0.127, 0.997, -1.163, 0.0001, 1.57, -1.57 ] common: <<: *NORMALLY tolerance: @@ -1187,9 +1220,10 @@ steps_list: - step: "middle pitch up" middle_pitch: target: *MIDDLE_PITCH_HIGHEST + delay: 0.3 - step: "arm ready" arm: - joints: [ 0.040,0.885,-2.107,*JOINT4_L90_POS,-0.00001, *JOINT6_R_90 ] + joints: [ 0.040,0.885,-2.107,*JOINT4_R90_POS,-0.00001, *JOINT6_HOME_POS ] common: <<: *QUICKLY tolerance: @@ -1203,7 +1237,7 @@ steps_list: target: -0.443 - step: "arm get gold" arm: - joints: [ 0.040,0.138,-1.649,*JOINT4_L90_POS,-0.00001, *JOINT6_R_90 ] + joints: [ 0.040,0.162,-1.414,*JOINT4_R90_POS, 0.097, *JOINT6_HOME_POS ] common: <<: *NORMALLY tolerance: @@ -1213,7 +1247,7 @@ steps_list: target: 0.055 - step: "arm lift gold" arm: - joints: [ 0.115,0.138,-1.649,*JOINT4_L90_POS,-0.00001, *JOINT6_R_90 ] + joints: [ 0.115,0.162,-1.414,*JOINT4_R90_POS,0.097, *JOINT6_HOME_POS ] common: <<: *NORMALLY tolerance: @@ -1229,7 +1263,7 @@ steps_list: <<: *FRONT_POS - step: "arm pull back gold" arm: - joints: [ 0.115,0.991,-2.142,*JOINT4_L90_POS,-0.335, *JOINT6_R_90 ] + joints: [ 0.115,0.988,-2.023,*JOINT4_R90_POS,0.233, *JOINT6_HOME_POS ] common: <<: *NORMALLY tolerance: From 965a94fd3798f15847cd86ed191d21533aa17114 Mon Sep 17 00:00:00 2001 From: cc0h <7921166012@qq.com> Date: Tue, 30 Jul 2024 22:50:48 +0800 Subject: [PATCH 69/80] Record. --- engineer_middleware/config/engineer2_steps_list.yaml | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/engineer_middleware/config/engineer2_steps_list.yaml b/engineer_middleware/config/engineer2_steps_list.yaml index fc615d9..cdb467f 100644 --- a/engineer_middleware/config/engineer2_steps_list.yaml +++ b/engineer_middleware/config/engineer2_steps_list.yaml @@ -139,7 +139,7 @@ common: joint6: home: &JOINT6_HOME_POS - 0.0 + 0.00001 l_90: &JOINT6_L_90 1.57 r_90: &JOINT6_R_90 @@ -1162,14 +1162,14 @@ steps_list: SIDE_BIG_ISLAND0: - step: "arm get gold" arm: - joints: [ 0.040,0.162,-1.414,*JOINT4_R90_POS, 0.097, *JOINT6_HOME_POS ] + joints: [ 0.040,0.16,-1.42,*JOINT4_R90_POS, 0.097, *JOINT6_HOME_POS ] common: <<: *NORMALLY tolerance: <<: *NORMAL_TOLERANCE - step: "arm lift gold" arm: - joints: [ 0.115,0.162,-1.414,*JOINT4_R90_POS,0.097, *JOINT6_HOME_POS ] + joints: [ 0.115,0.16,-1.42,*JOINT4_R90_POS,0.097, *JOINT6_HOME_POS ] common: <<: *NORMALLY tolerance: @@ -1179,7 +1179,7 @@ steps_list: <<: *FRONT_POS - step: "arm pull back gold" arm: - joints: [ 0.115,0.988,-2.023,*JOINT4_R90_POS,0.233, *JOINT6_HOME_POS ] + joints: [ 0.114,0.988,-2.023,*JOINT4_R90_POS,0.232, *JOINT6_HOME_POS ] common: <<: *NORMALLY tolerance: From 772ddf1e92ee1e70c216077a640443fe444d9d9b Mon Sep 17 00:00:00 2001 From: cc0h <7921166012@qq.com> Date: Wed, 31 Jul 2024 08:24:02 +0800 Subject: [PATCH 70/80] Enable max acceleration limit for engineer2. --- engineer2_arm_config/config/joint_limits.yaml | 30 +++++++++---------- 1 file changed, 15 insertions(+), 15 deletions(-) diff --git a/engineer2_arm_config/config/joint_limits.yaml b/engineer2_arm_config/config/joint_limits.yaml index 75658e6..7031779 100644 --- a/engineer2_arm_config/config/joint_limits.yaml +++ b/engineer2_arm_config/config/joint_limits.yaml @@ -2,39 +2,39 @@ # For beginners, we downscale velocity and acceleration limits. # You can always specify higher scaling factors (<= 1.0) in your motion requests. # Increase the values below to 1.0 to always move at maximum speed. -default_velocity_scaling_factor: 1.0 -default_acceleration_scaling_factor: 1.0 +default_velocity_scaling_factor: 0.1 +default_acceleration_scaling_factor: 0.1 # Specific joint properties can be changed with the keys [max_position, min_position, max_velocity, max_acceleration] # Joint limits can be turned off with [has_velocity_limits, has_acceleration_limits] joint_limits: joint1: has_velocity_limits: true - max_velocity: 2.25 - has_acceleration_limits: false - max_acceleration: 0 + max_velocity: 1.25 + has_acceleration_limits: true + max_acceleration: 1 joint2: has_velocity_limits: true max_velocity: 10.46 - has_acceleration_limits: false - max_acceleration: 0 + has_acceleration_limits: true + max_acceleration: 10 joint3: has_velocity_limits: true max_velocity: 5.44 - has_acceleration_limits: false - max_acceleration: 0 + has_acceleration_limits: true + max_acceleration: 15 joint4: has_velocity_limits: true max_velocity: 20.94 - has_acceleration_limits: false - max_acceleration: 0 + has_acceleration_limits: true + max_acceleration: 15 joint5: has_velocity_limits: true max_velocity: 10 - has_acceleration_limits: false - max_acceleration: 0 + has_acceleration_limits: true + max_acceleration: 15 joint6: has_velocity_limits: true max_velocity: 10 - has_acceleration_limits: false - max_acceleration: 0 + has_acceleration_limits: true + max_acceleration: 15 From 48e274c7e8e02851b1a43e6def51b077c6cda8f3 Mon Sep 17 00:00:00 2001 From: 2194555 <3631676002@qq.com> Date: Wed, 31 Jul 2024 22:30:52 +0800 Subject: [PATCH 71/80] Update. --- .../config/engineer_steps_list.yaml | 32 +++++++++++++------ 1 file changed, 23 insertions(+), 9 deletions(-) diff --git a/engineer_middleware/config/engineer_steps_list.yaml b/engineer_middleware/config/engineer_steps_list.yaml index 92a1e58..257570e 100644 --- a/engineer_middleware/config/engineer_steps_list.yaml +++ b/engineer_middleware/config/engineer_steps_list.yaml @@ -42,7 +42,7 @@ common: bin_get_stone_left: &JOINT1_BIN_GET_STONE_LEFT 0.2 bin_get_stone_right: &JOINT1_BIN_GET_STONE_RIGHT - 0.1198 + 0.1248 big_ready: &JOINT1_BIG_READY 0.005 big_adjust_mid: &JOINT1_BIG_ADJUST_MID @@ -80,9 +80,9 @@ common: small_store: &JOINT2_SMALL_STORE 0.060290 bin_get_stone: &JOINT2_BIN_GET_STONE - 0.13 + 0.1 back_bin_position: &JOINT2_BACK_BIN_POSITION - 0.06451 + 0.04551 avoid_collision: &JOINT2_AVOID_COLLISION 0.1 big_ready: &JOINT2_BIG_READY @@ -113,7 +113,7 @@ common: small_turn: &JOINT3_SMALL_TURN 0.257 bin_get_stone: &JOINT3_BIN_GET_STONE - 0.2608 + 0.2528 bin_get_lv4_stone: &JOINT3_BIN_GET_LV4_STONE 0.294414 big_ready: &JOINT3_BIG_READY @@ -160,7 +160,7 @@ common: right_90_position: &JOINT5_R90_POSITION 1.59 bin_get_stone: &JOINT5_BIN_GET_STONE - 1.63912 + 1.53912 big_ready: &JOINT5_BIG_READY -0.017 big_ready_store: &JOINT5_BIG_READY_STORE @@ -191,7 +191,7 @@ common: small_ready_store: &JOINT6_SMALL_READY_STORE -1.633 bin_get_stone: &JOINT6_BIN_GET_STONE - -1.5936 + -1.6036 big_ready: &JOINT6_BIG_READY 0.014 big_adjust_mid: &JOINT6_BIG_ADJUST_MID @@ -216,7 +216,7 @@ common: big_ready_store: &JOINT7_BIG_READY_STORE -1.522 small_ready: &JOINT7_SMALL_READY - 0.012 + -0.083 small_ready_store: &JOINT7_SMALL_READY_STORE 1.722 exchange: &JOINT7_EXCHANGE @@ -224,7 +224,7 @@ common: lv4_ore: &JOINT7_LV4_ORE -0.098300 bin_get_left: &JOINT7_BIN_GET_LEFT - -1.6973 + -1.5793 to: &JOINT7_TAKE_ONE -0.844500 bin_get_right: &JOINT7_BIN_GET_RIGHT @@ -243,7 +243,12 @@ common: small_island_pos: &BIG_ISLAND_POS frame: gimbal_lifter position: [ 1., -1., 0.] - + test: &GIMBAL_TEST + frame: link4 + position: [1., 0., 0.] + test2: &GIMBAL_TEST2 + frame: link7 + position: [0., 0., -1.] gimbal_lifter: button_pos: &BUTTON_POS @@ -363,6 +368,15 @@ common: 2.6 steps_list: + TEST1: + - step: "gimbal test1" + gimbal: + <<: *GIMBAL_TEST + TEST2: + - step: "gimbal test2" + gimbal: + <<: *GIMBAL_TEST2 + EXCHANGE_POS: - step: "gimbal look front" gimbal: From 38a8c64924e8e637ae8d76875fb1b77d97ca8b05 Mon Sep 17 00:00:00 2001 From: cc0h <7921166012@qq.com> Date: Thu, 1 Aug 2024 04:28:55 +0800 Subject: [PATCH 72/80] Modify gpio motion. --- .../include/engineer_middleware/motion.h | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/engineer_middleware/include/engineer_middleware/motion.h b/engineer_middleware/include/engineer_middleware/motion.h index b2bdf94..eec089f 100644 --- a/engineer_middleware/include/engineer_middleware/motion.h +++ b/engineer_middleware/include/engineer_middleware/motion.h @@ -529,8 +529,8 @@ class GpioMotion : public PublishMotion : PublishMotion(motion, interface) { delay_ = xmlRpcGetDouble(motion, "delay", 0.01); - msg_.gpio_state.assign(8, false); - msg_.gpio_name.assign(8, "no_registered"); + msg_.gpio_state.assign(6, false); + msg_.gpio_name.assign(6, "no_registered"); pin_ = motion["pin"]; state_ = motion["state"]; switch (pin_) @@ -551,15 +551,15 @@ class GpioMotion : public PublishMotion msg_.gpio_name[4] = "gold_gripper"; break; case 5: - msg_.gpio_name[5] = "unused"; - ROS_WARN_STREAM("GPIO port 6 is unused now!"); + msg_.gpio_name[5] = "silver_pump"; break; case 6: msg_.gpio_name[6] = "unused"; ROS_WARN_STREAM("GPIO port 7 is unused now!"); break; case 7: - msg_.gpio_name[7] = "silver_pump"; + msg_.gpio_name[7] = "unused"; + ROS_WARN_STREAM("GPIO port 7 is unused now!"); break; } } From fd21d7b26fef71d325a5225177a637dfa9a36ce3 Mon Sep 17 00:00:00 2001 From: cc0h <7921166012@qq.com> Date: Thu, 1 Aug 2024 04:29:14 +0800 Subject: [PATCH 73/80] Record. --- .../config/engineer2_steps_list.yaml | 138 +++++++++++------- 1 file changed, 89 insertions(+), 49 deletions(-) diff --git a/engineer_middleware/config/engineer2_steps_list.yaml b/engineer_middleware/config/engineer2_steps_list.yaml index cdb467f..8fa6918 100644 --- a/engineer_middleware/config/engineer2_steps_list.yaml +++ b/engineer_middleware/config/engineer2_steps_list.yaml @@ -1,16 +1,16 @@ common: speed: slowly: &SLOWLY - speed: 0.4 + speed: 0.3 accel: 0.1 timeout: 15. normally: &NORMALLY - speed: 2.5 - accel: 1.5 + speed: 0.6 + accel: 0.6 timeout: 6 quickly: &QUICKLY - speed: 3 - accel: 2 + speed: 1 + accel: 1 timeout: 6. tolerance: @@ -39,7 +39,7 @@ common: get_stored: &JOINT1_GET_STORED_POS 0.478 get_stored_gold: &JOINT1_GET_STORED_GOLD_POS - 0.05 + 0.04 ready_store: &JOINT1_READY_STORE_POS 0.44 store: &JOINT1_STORE_POS @@ -76,7 +76,7 @@ common: get_stored_silver1: &JOINT2_GET_STORED_S1_POS -3.833 get_stored_gold: &JOINT2_GET_STORED_GOLD_POS - -1.062 + -1.257 joint3: home: &JOINT3_HOME_POS @@ -108,7 +108,7 @@ common: ready_get_stored_gold: &JOINT3_READY_GET_STORED_GOLD_POS -1.67 get_stored_gold: &JOINT3_GET_STORED_GOLD_POS - -1.92 + -1.75 joint4: home: &JOINT4_HOME_POS @@ -119,6 +119,8 @@ common: -1.57 l90: &JOINT4_L90_POS 1.57 + get_stored_gold: &JOINT4_GET_STORED_GOLD_POS + -1.721 joint5: @@ -129,7 +131,7 @@ common: down_90: &JOINT5_DOWN_90 1.57 get_stored_gold: &JOINT5_GET_STORED_GOLD_POS - 1.586 + 1.726 store_to_s1: &JOINT5_STORE_TO_S1_POS -0.215 store_to_s2: &JOINT5_STORE_TO_S2_POS @@ -180,14 +182,14 @@ common: tolerance: <<: *NORMAL_TOLERANCE - lv5_r_exchange: &LV5_L_EXCHANGE + lv5_r_exchange: &LV5_R_EXCHANGE joints: [ *JOINT1_EXCHANGE_POS, *JOINT2_L_EXCHANGE_POS, *JOINT3_L_EXCHANGE_POS, *JOINT4_R90_POS, *JOINT5_DOWN_90, *JOINT6_L_90] common: <<: *NORMALLY tolerance: <<: *NORMAL_TOLERANCE - lv5_l_exchange: &LV5_R_EXCHANGE + lv5_l_exchange: &LV5_L_EXCHANGE joints: [ *JOINT1_EXCHANGE_POS, *JOINT2_R_EXCHANGE_POS, *JOINT3_R_EXCHANGE_POS, *JOINT4_L90_POS, *JOINT5_DOWN_90, *JOINT6_R_90] common: <<: *NORMALLY @@ -271,10 +273,10 @@ common: pin: 4 state: false open_silver_gripper: &OPEN_S_GRIPPER - pin: 7 + pin: 5 state: true close_silver_gripper: &CLOSE_S_GRIPPER - pin: 7 + pin: 5 state: false @@ -400,6 +402,7 @@ steps_list: - step: "middle pitch up" middle_pitch: target: *MIDDLE_PITCH_NORMAL + delay: 0.3 - step: "arm enter exchange pos" arm: <<: *LV4_EXCHANGE @@ -410,6 +413,7 @@ steps_list: - step: "middle pitch up" middle_pitch: target: *MIDDLE_PITCH_NORMAL + delay: 0.3 - step: "arm enter exchange pos" arm: <<: *LV5_L_EXCHANGE @@ -421,6 +425,7 @@ steps_list: - step: "middle pitch up" middle_pitch: target: *MIDDLE_PITCH_NORMAL + delay: 0.3 - step: "arm enter exchange pos" arm: <<: *LV5_R_EXCHANGE @@ -428,42 +433,47 @@ steps_list: gimbal: <<: *RIGHT_POS - LV5_R_CAR_FRONT_EXCHANGE: + LV5_R_DROP_GOLD_EXCHANGE: - step: "middle pitch up" middle_pitch: target: *MIDDLE_PITCH_NORMAL delay: 0.3 + - step: "drop side gold" + gripper: + <<: *CLOSE_GOLD + - step: "add stone" + stone_num: + change: "-g" - step: "arm enter exchange pos" arm: - joints: [ *JOINT1_EXCHANGE_POS, -0.774, *JOINT3_R_EXCHANGE_POS, *JOINT4_L90_POS, *JOINT5_DOWN_90, *JOINT6_R_90 ] - common: - <<: *NORMALLY - tolerance: - <<: *NORMAL_TOLERANCE + <<: *LV5_R_EXCHANGE - step: "gimbal ready" gimbal: - <<: *FRONT_POS + <<: *RIGHT_POS - LV5_L_CAR_FRONT_EXCHANGE: + LV5_L_DROP_GOLD_EXCHANGE: - step: "middle pitch up" middle_pitch: target: *MIDDLE_PITCH_NORMAL delay: 0.3 + - step: "drop side gold" + gripper: + <<: *CLOSE_GOLD + - step: "add stone" + stone_num: + change: "-g" - step: "arm enter exchange pos" arm: - joints: [ *JOINT1_EXCHANGE_POS, 1.001, *JOINT3_L_EXCHANGE_POS, *JOINT4_R90_POS, *JOINT5_DOWN_90, *JOINT6_L_90 ] - common: - <<: *NORMALLY - tolerance: - <<: *NORMAL_TOLERANCE + <<: *LV5_L_EXCHANGE - step: "gimbal ready" gimbal: - <<: *FRONT_POS + <<: *RIGHT_POS GET_SMALL_ISLAND: - step: "middle pitch up" middle_pitch: target: *MIDDLE_PITCH_NORMAL + delay: 0.3 - step: "gimbal look right" gimbal: <<: *RIGHT_POS @@ -632,6 +642,7 @@ steps_list: - step: "middle pitch up" middle_pitch: target: *MIDDLE_PITCH_NORMAL + delay: 0.3 - step: "arm ready get ground stone" arm: joints: [*JOINT1_GET_GROUND_POS, 0.94, -1.266, *JOINT4_ZERO_POS, 1.112 , -0.397] @@ -671,6 +682,7 @@ steps_list: - step: "middle pitch up" middle_pitch: target: *MIDDLE_PITCH_NORMAL + delay: 0.3 - step: "ready to get silver from gripper1" arm: joints: [*JOINT1_READY_GET_STORED_POS, *JOINT2_GET_STORED_S1_POS, *JOINT3_GET_STORED_SILVER1_POS, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_GET_S1] @@ -714,6 +726,7 @@ steps_list: - step: "middle pitch up" middle_pitch: target: *MIDDLE_PITCH_NORMAL + delay: 0.3 - step: "ready to get silver from gripper2" arm: joints: [ *JOINT1_READY_GET_STORED_POS, *JOINT2_GET_STORED_S2_POS, *JOINT3_GET_STORED_SILVER2_POS, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_GET_S2 ] @@ -756,6 +769,7 @@ steps_list: - step: "middle pitch up" middle_pitch: target: *MIDDLE_PITCH_NORMAL + delay: 0.3 - step: "lift arm" arm: joints: [ *JOINT1_READY_GET_STORED_POS, 0.113, -2.063, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_GET_S3 ] @@ -802,19 +816,23 @@ steps_list: <<: *NORMAL_TOLERANCE GET_STORED_GOLD: + - step: "middle pitch up" + middle_pitch: + target: *MIDDLE_PITCH_NORMAL + delay: 0.3 + - step: "open main gripper" + gripper: + <<: *OPEN_MAIN_GRIPPER - step: "ready to get stored gold" arm: - joints: [ *JOINT1_GET_STORED_GOLD_POS, *JOINT2_GET_STORED_GOLD_POS, *JOINT3_READY_GET_STORED_GOLD_POS, *JOINT4_R90_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ] + joints: [ *JOINT1_GET_STORED_GOLD_POS, *JOINT2_GET_STORED_GOLD_POS, *JOINT3_READY_GET_STORED_GOLD_POS, *JOINT4_R90_POS, *JOINT5_DOWN_90, *JOINT6_L_90 ] common: <<: *NORMALLY tolerance: <<: *NORMAL_TOLERANCE - - step: "open main gripper" - gripper: - <<: *OPEN_MAIN_GRIPPER - step: "get stored gold" arm: - joints: [ *JOINT1_GET_STORED_GOLD_POS, *JOINT2_GET_STORED_GOLD_POS, *JOINT3_GET_STORED_GOLD_POS, *JOINT4_R90_POS, *JOINT5_GET_STORED_GOLD_POS, *JOINT6_HOME_POS] + joints: [ *JOINT1_GET_STORED_GOLD_POS, *JOINT2_GET_STORED_GOLD_POS, *JOINT3_GET_STORED_GOLD_POS, *JOINT4_GET_STORED_GOLD_POS, *JOINT5_GET_STORED_GOLD_POS, *JOINT6_L_90] common: <<: *NORMALLY tolerance: @@ -822,19 +840,22 @@ steps_list: - step: "close gold gripper" gripper: <<: *CLOSE_GOLD + - step: "move gold gripper back" + gold_pusher: + target: 0.00001 - step: "add stone" stone_num: change: "+g" - step: "move out gold" arm: - joints: [ *JOINT1_GET_STORED_GOLD_POS, *JOINT2_GET_STORED_GOLD_POS, *JOINT3_READY_GET_STORED_GOLD_POS, *JOINT4_R90_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ] + joints: [ *JOINT1_GET_STORED_GOLD_POS, *JOINT2_GET_STORED_GOLD_POS, *JOINT3_READY_GET_STORED_GOLD_POS, *JOINT4_R90_POS, *JOINT5_DOWN_90, *JOINT6_L_90 ] common: <<: *NORMALLY tolerance: <<: *BIGGER_TOLERANCE - step: "lift up gold" arm: - joints: [ *JOINT1_EXCHANGE_POS, *JOINT2_GET_STORED_GOLD_POS, *JOINT3_READY_GET_STORED_GOLD_POS, *JOINT4_R90_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ] + joints: [ *JOINT1_EXCHANGE_POS, *JOINT2_GET_STORED_GOLD_POS, *JOINT3_READY_GET_STORED_GOLD_POS, *JOINT4_R90_POS, *JOINT5_DOWN_90, *JOINT6_L_90 ] common: <<: *NORMALLY tolerance: @@ -1118,7 +1139,7 @@ steps_list: - step: "get gold" gold_pusher: target: -0.443 - delay: 0.5 + delay: 1.2 - step: "gimbal look front" gimbal: <<: *FRONT_POS @@ -1129,7 +1150,7 @@ steps_list: - step: "pull back gold" gold_pusher: target: -0.054 - delay: 0.6 + delay: 1 - step: "chassis left" chassis: <<: *CHASSIS_MOVE_LEFT @@ -1139,15 +1160,12 @@ steps_list: SIDE_BIG_ISLAND: - - step: "middle pitch up" - middle_pitch: - target: *MIDDLE_PITCH_NORMAL - step: "gimbal look right" gimbal: <<: *RIGHT_POS - step: "middle pitch up" middle_pitch: - target: *MIDDLE_PITCH_HIGHEST + target: *MIDDLE_PITCH_NORMAL delay: 0.3 - step: "arm ready" arm: @@ -1162,16 +1180,16 @@ steps_list: SIDE_BIG_ISLAND0: - step: "arm get gold" arm: - joints: [ 0.040,0.16,-1.42,*JOINT4_R90_POS, 0.097, *JOINT6_HOME_POS ] + joints: [ 0.040,0.147,-1.429,*JOINT4_R90_POS, 0.08, *JOINT6_HOME_POS ] common: <<: *NORMALLY tolerance: <<: *NORMAL_TOLERANCE - step: "arm lift gold" arm: - joints: [ 0.115,0.16,-1.42,*JOINT4_R90_POS,0.097, *JOINT6_HOME_POS ] + joints: [ 0.114,0.147,-1.429,*JOINT4_R90_POS, 0.08, *JOINT6_HOME_POS ] common: - <<: *NORMALLY + <<: *SLOWLY tolerance: <<: *NORMAL_TOLERANCE - step: "gimbal look front" @@ -1179,9 +1197,9 @@ steps_list: <<: *FRONT_POS - step: "arm pull back gold" arm: - joints: [ 0.114,0.988,-2.023,*JOINT4_R90_POS,0.232, *JOINT6_HOME_POS ] + joints: [ 0.114,0.988,-1.927, *JOINT4_R90_POS,0.344, *JOINT6_HOME_POS ] common: - <<: *NORMALLY + <<: *SLOWLY tolerance: <<: *BIGGER_TOLERANCE - step: "chassis left" @@ -1199,6 +1217,7 @@ steps_list: - step: "middle pitch up" middle_pitch: target: *MIDDLE_PITCH_NORMAL + delay: 0.3 - step: "gimbal look right" gimbal: <<: *RIGHT_POS @@ -1237,7 +1256,7 @@ steps_list: target: -0.443 - step: "arm get gold" arm: - joints: [ 0.040,0.162,-1.414,*JOINT4_R90_POS, 0.097, *JOINT6_HOME_POS ] + joints: [ 0.040,0.147,-1.429,*JOINT4_R90_POS, 0.08, *JOINT6_HOME_POS ] common: <<: *NORMALLY tolerance: @@ -1247,7 +1266,7 @@ steps_list: target: 0.055 - step: "arm lift gold" arm: - joints: [ 0.115,0.162,-1.414,*JOINT4_R90_POS,0.097, *JOINT6_HOME_POS ] + joints: [ 0.114,0.147,-1.429,*JOINT4_R90_POS, 0.08, *JOINT6_HOME_POS ] common: <<: *NORMALLY tolerance: @@ -1263,9 +1282,9 @@ steps_list: <<: *FRONT_POS - step: "arm pull back gold" arm: - joints: [ 0.115,0.988,-2.023,*JOINT4_R90_POS,0.233, *JOINT6_HOME_POS ] + joints: [ 0.114,0.988,-1.927, *JOINT4_R90_POS,0.344, *JOINT6_HOME_POS ] common: - <<: *NORMALLY + <<: *SLOWLY tolerance: <<: *BIGGER_TOLERANCE - step: "chassis left" @@ -1372,6 +1391,7 @@ steps_list: - step: "middle pitch up" middle_pitch: target: *MIDDLE_PITCH_NORMAL + delay: 0.3 - step: "ready to get silver from gripper1" arm: joints: [ *JOINT1_READY_GET_STORED_POS, *JOINT2_GET_STORED_S1_POS, *JOINT3_GET_STORED_SILVER1_POS, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_GET_S1 ] @@ -1419,6 +1439,7 @@ steps_list: - step: "middle pitch up" middle_pitch: target: *MIDDLE_PITCH_NORMAL + delay: 0.3 - step: "ready to get silver from gripper2" arm: joints: [ *JOINT1_READY_GET_STORED_POS, *JOINT2_GET_STORED_S2_POS, *JOINT3_GET_STORED_SILVER2_POS, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_GET_S2 ] @@ -1462,6 +1483,7 @@ steps_list: - step: "middle pitch up" middle_pitch: target: *MIDDLE_PITCH_NORMAL + delay: 0.3 - step: "lift arm" arm: joints: [ *JOINT1_READY_GET_STORED_POS, 0.113, -2.063, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ] @@ -1509,6 +1531,7 @@ steps_list: - step: "middle pitch up" middle_pitch: target: *MIDDLE_PITCH_NORMAL + delay: 0.3 - step: "ready to get stored gold" arm: joints: [ *JOINT1_GET_STORED_GOLD_POS, *JOINT2_GET_STORED_GOLD_POS, *JOINT3_READY_GET_STORED_GOLD_POS, *JOINT4_R90_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ] @@ -1529,6 +1552,9 @@ steps_list: - step: "close gold gripper" gripper: <<: *CLOSE_GOLD + - step: "move gold gripper back" + gold_pusher: + target: 0.00001 - step: "add stone" stone_num: change: "-g" @@ -1556,6 +1582,7 @@ steps_list: - step: "middle pitch up" middle_pitch: target: *MIDDLE_PITCH_NORMAL + delay: 0.3 - step: "ready to get silver from gripper1" arm: joints: [ *JOINT1_READY_GET_STORED_POS, *JOINT2_GET_STORED_S1_POS, *JOINT3_GET_STORED_SILVER1_POS, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_GET_S1 ] @@ -1600,6 +1627,7 @@ steps_list: - step: "middle pitch up" middle_pitch: target: *MIDDLE_PITCH_NORMAL + delay: 0.3 - step: "ready to get silver from gripper2" arm: joints: [ *JOINT1_READY_GET_STORED_POS, *JOINT2_GET_STORED_S2_POS, *JOINT3_GET_STORED_SILVER2_POS, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_GET_S2 ] @@ -1640,6 +1668,7 @@ steps_list: - step: "middle pitch up" middle_pitch: target: *MIDDLE_PITCH_NORMAL + delay: 0.3 - step: "lift arm" arm: joints: [ *JOINT1_READY_GET_STORED_POS, 0.113, -2.063, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ] @@ -1687,6 +1716,7 @@ steps_list: - step: "middle pitch up" middle_pitch: target: *MIDDLE_PITCH_NORMAL + delay: 0.3 - step: "ready to get stored gold" arm: joints: [ *JOINT1_GET_STORED_GOLD_POS, *JOINT2_GET_STORED_GOLD_POS, *JOINT3_READY_GET_STORED_GOLD_POS, *JOINT4_R90_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ] @@ -1707,6 +1737,9 @@ steps_list: - step: "close gold gripper" gripper: <<: *CLOSE_GOLD + - step: "move gold gripper back" + gold_pusher: + target: 0.00001 - step: "add stone" stone_num: change: "-g" @@ -1734,6 +1767,7 @@ steps_list: - step: "middle pitch up" middle_pitch: target: *MIDDLE_PITCH_NORMAL + delay: 0.3 - step: "ready to get silver from gripper1" arm: joints: [ *JOINT1_READY_GET_STORED_POS, *JOINT2_GET_STORED_S1_POS, *JOINT3_GET_STORED_SILVER1_POS, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_GET_S1 ] @@ -1778,6 +1812,7 @@ steps_list: - step: "middle pitch up" middle_pitch: target: *MIDDLE_PITCH_NORMAL + delay: 0.3 - step: "ready to get silver from gripper2" arm: joints: [ *JOINT1_READY_GET_STORED_POS, *JOINT2_GET_STORED_S2_POS, *JOINT3_GET_STORED_SILVER2_POS, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_GET_S2 ] @@ -1818,6 +1853,7 @@ steps_list: - step: "middle pitch up" middle_pitch: target: *MIDDLE_PITCH_NORMAL + delay: 0.3 - step: "lift arm" arm: joints: [ *JOINT1_READY_GET_STORED_POS, 0.113, -2.063, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ] @@ -1865,6 +1901,7 @@ steps_list: - step: "middle pitch up" middle_pitch: target: *MIDDLE_PITCH_NORMAL + delay: 0.3 - step: "ready to get stored gold" arm: joints: [ *JOINT1_GET_STORED_GOLD_POS, *JOINT2_GET_STORED_GOLD_POS, *JOINT3_READY_GET_STORED_GOLD_POS, *JOINT4_R90_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ] @@ -1885,6 +1922,9 @@ steps_list: - step: "close gold gripper" gripper: <<: *CLOSE_GOLD + - step: "move gold gripper back" + gold_pusher: + target: 0.00001 - step: "add stone" stone_num: change: "-g" From b4ad7edd48bb98adc9c313c47884e66dd602a197 Mon Sep 17 00:00:00 2001 From: cc0h <7921166012@qq.com> Date: Thu, 1 Aug 2024 08:53:06 +0800 Subject: [PATCH 74/80] Swap exchange direction of engineer2. --- ...al_motion_planner_planning_pipeline.launch.xml | 15 --------------- .../config/engineer2_steps_list.yaml | 11 ++++++----- 2 files changed, 6 insertions(+), 20 deletions(-) delete mode 100644 engineer2_arm_config/launch/pilz_industrial_motion_planner_planning_pipeline.launch.xml diff --git a/engineer2_arm_config/launch/pilz_industrial_motion_planner_planning_pipeline.launch.xml b/engineer2_arm_config/launch/pilz_industrial_motion_planner_planning_pipeline.launch.xml deleted file mode 100644 index c7c4cf5..0000000 --- a/engineer2_arm_config/launch/pilz_industrial_motion_planner_planning_pipeline.launch.xml +++ /dev/null @@ -1,15 +0,0 @@ - - - - - - - - - - - - - - diff --git a/engineer_middleware/config/engineer2_steps_list.yaml b/engineer_middleware/config/engineer2_steps_list.yaml index 8fa6918..cd18ea2 100644 --- a/engineer_middleware/config/engineer2_steps_list.yaml +++ b/engineer_middleware/config/engineer2_steps_list.yaml @@ -182,14 +182,14 @@ common: tolerance: <<: *NORMAL_TOLERANCE - lv5_r_exchange: &LV5_R_EXCHANGE + lv5_r_exchange: &LV5_L_EXCHANGE joints: [ *JOINT1_EXCHANGE_POS, *JOINT2_L_EXCHANGE_POS, *JOINT3_L_EXCHANGE_POS, *JOINT4_R90_POS, *JOINT5_DOWN_90, *JOINT6_L_90] common: <<: *NORMALLY tolerance: <<: *NORMAL_TOLERANCE - lv5_l_exchange: &LV5_L_EXCHANGE + lv5_l_exchange: &LV5_R_EXCHANGE joints: [ *JOINT1_EXCHANGE_POS, *JOINT2_R_EXCHANGE_POS, *JOINT3_R_EXCHANGE_POS, *JOINT4_L90_POS, *JOINT5_DOWN_90, *JOINT6_R_90] common: <<: *NORMALLY @@ -1151,12 +1151,13 @@ steps_list: gold_pusher: target: -0.054 delay: 1 - - step: "chassis left" - chassis: - <<: *CHASSIS_MOVE_LEFT - step: "add stone" stone_num: change: "+g" + - step: "chassis left" + chassis: + <<: *CHASSIS_MOVE_LEFT + SIDE_BIG_ISLAND: From 06de8595cf8f2e0f81d1eebd439035db9069423a Mon Sep 17 00:00:00 2001 From: cc0h <7921166012@qq.com> Date: Mon, 5 Aug 2024 23:26:59 +0800 Subject: [PATCH 75/80] Modify get small island motion. --- engineer2_arm_config/launch/gazebo.launch | 34 ---- engineer2_arm_config/launch/moveit.rviz | 16 +- ...ntrol_moveit_controller_manager.launch.xml | 4 - .../launch/ros_controllers.launch | 11 -- .../config/engineer2_steps_list.yaml | 185 ++++++++++++------ 5 files changed, 135 insertions(+), 115 deletions(-) delete mode 100644 engineer2_arm_config/launch/gazebo.launch delete mode 100644 engineer2_arm_config/launch/ros_control_moveit_controller_manager.launch.xml delete mode 100644 engineer2_arm_config/launch/ros_controllers.launch diff --git a/engineer2_arm_config/launch/gazebo.launch b/engineer2_arm_config/launch/gazebo.launch deleted file mode 100644 index 9f7e235..0000000 --- a/engineer2_arm_config/launch/gazebo.launch +++ /dev/null @@ -1,34 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/engineer2_arm_config/launch/moveit.rviz b/engineer2_arm_config/launch/moveit.rviz index 09ce292..e70752e 100644 --- a/engineer2_arm_config/launch/moveit.rviz +++ b/engineer2_arm_config/launch/moveit.rviz @@ -354,17 +354,17 @@ Visualization Manager: Value: false Field of View: 0.75 Focal Point: - X: -0.27402463555336 - Y: -0.39333611726760864 - Z: 0.4662781059741974 + X: -0.3085601031780243 + Y: 0.24779054522514343 + Z: 0.4662521481513977 Focal Shape Fixed Size: true Focal Shape Size: 0.05000000074505806 Invert Z Axis: false Name: Current View Near Clip Distance: 0.009999999776482582 - Pitch: 1.419796109199524 + Pitch: 1.5697963237762451 Target Frame: base_link - Yaw: 3.614947557449341 + Yaw: 3.154945135116577 Saved: ~ Window Geometry: Displays: @@ -374,11 +374,7 @@ Window Geometry: collapsed: false Hide Left Dock: false Hide Right Dock: false - MotionPlanning: - collapsed: false - MotionPlanning - Trajectory Slider: - collapsed: false - QMainWindow State: 000000ff00000000fd0000000100000000000001f3000005a6fc0200000007fb000000100044006900730070006c006100790073010000003d000005a6000000c900fffffffb0000000800480065006c00700000000342000000bb0000006e00fffffffb0000000a00560069006500770073000000010c000000a4000000a400fffffffb0000000c00430061006d00650072006100000002ff000001610000000000000000fb0000001e004d006f00740069006f006e00200050006c0061006e006e0069006e00670100000374000001890000000000000000fb00000044004d006f00740069006f006e0050006c0061006e006e0069006e00670020002d0020005400720061006a006500630074006f0072007900200053006c00690064006500720000000000ffffffff0000001600000016fb0000001c004d006f00740069006f006e0050006c0061006e006e0069006e006700000002f4000002ef0000017d00ffffff000007bf000005a600000001000000020000000100000002fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + QMainWindow State: 000000ff00000000fd0000000100000000000001f3000005a6fc0200000007fb000000100044006900730070006c006100790073010000003d000005a6000000c900fffffffb0000000800480065006c00700000000342000000bb0000006e00fffffffb0000000a00560069006500770073000000010c000000a4000000a400fffffffb0000000c00430061006d00650072006100000002ff000001610000000000000000fb0000001e004d006f00740069006f006e00200050006c0061006e006e0069006e00670100000374000001890000000000000000fb00000044004d006f00740069006f006e0050006c0061006e006e0069006e00670020002d0020005400720061006a006500630074006f0072007900200053006c00690064006500720000000000ffffffff0000000000000000fb0000001c004d006f00740069006f006e0050006c0061006e006e0069006e006700000002f4000002ef0000000000000000000007bf000005a600000001000000020000000100000002fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 Views: collapsed: false Width: 2488 diff --git a/engineer2_arm_config/launch/ros_control_moveit_controller_manager.launch.xml b/engineer2_arm_config/launch/ros_control_moveit_controller_manager.launch.xml deleted file mode 100644 index 9ebc91c..0000000 --- a/engineer2_arm_config/launch/ros_control_moveit_controller_manager.launch.xml +++ /dev/null @@ -1,4 +0,0 @@ - - - - diff --git a/engineer2_arm_config/launch/ros_controllers.launch b/engineer2_arm_config/launch/ros_controllers.launch deleted file mode 100644 index f149647..0000000 --- a/engineer2_arm_config/launch/ros_controllers.launch +++ /dev/null @@ -1,11 +0,0 @@ - - - - - - - - - - diff --git a/engineer_middleware/config/engineer2_steps_list.yaml b/engineer_middleware/config/engineer2_steps_list.yaml index cd18ea2..f280a96 100644 --- a/engineer_middleware/config/engineer2_steps_list.yaml +++ b/engineer_middleware/config/engineer2_steps_list.yaml @@ -33,7 +33,7 @@ common: ready_get_small: &JOINT1_READY_GET_SMALL_POS 0.42 get_small: &JOINT1_GET_SMALL_POS - 0.3 + 0.34 ready_get_stored: &JOINT1_READY_GET_STORED_POS 0.54 get_stored: &JOINT1_GET_STORED_POS @@ -58,7 +58,7 @@ common: r_exchange: &JOINT2_R_EXCHANGE_POS -2.5 get_small_island_l: &JOINT2_GET_SMALL_ISLAND_L_POS - -0.098 + 0.858 get_small_island_m: &JOINT2_GET_SMALL_ISLAND_M_POS -1.101 get_small_island_r: &JOINT2_GET_SMALL_ISLAND_R_POS @@ -88,7 +88,7 @@ common: r_exchange: &JOINT3_R_EXCHANGE_POS 0.7 get_small_island_l: &JOINT3_GET_SMALL_ISLAND_L_POS - -1.948 + -1.643 get_small_island_m: &JOINT3_GET_SMALL_ISLAND_M_POS -1.833 get_small_island_r: &JOINT3_GET_SMALL_ISLAND_R_POS @@ -151,13 +151,13 @@ common: get_small2: &JOINT6_SMALL2 4.387 get_small1: &JOINT6_SMALL1 - 2.7 - get_s3: &JOINT6_GET_S3 - -0.9 - get_s2: &JOINT6_GET_S2 - -0.346 - get_s1: &JOINT6_GET_S1 - -0.65 + -0.481 + get_own_s3: &JOINT6_GET_S3 + 0.6536 + get_own_s2: &JOINT6_GET_S2 + 1.22727 + get_own_s1: &JOINT6_GET_S1 + 0.9794 middle_pitch: ground_stone: &MIDDLE_PITCH_GROUND_STONE @@ -210,7 +210,7 @@ common: frame: base_link position: [ -3., -0.3, 0. ] follow_ee: &FOLLOW_EE - frame: base_link + frame: link6 position: [ 0.15, 0.00, 0. ] chassis_forward: &CHASSIS_FORWARD @@ -240,6 +240,15 @@ common: common: timeout: 3. + chassis_move_back: &CHASSIS_MOVE_BACK + frame: base_link + position: [ -0.4, 0.0 ] + yaw: 0.0 + chassis_tolerance_position: 0.1 + chassis_tolerance_angular: 0.2 + common: + timeout: 3. + gripper: open_main_gripper: &OPEN_MAIN_GRIPPER @@ -474,9 +483,9 @@ steps_list: middle_pitch: target: *MIDDLE_PITCH_NORMAL delay: 0.3 - - step: "gimbal look right" + - step: "gimbal look front" gimbal: - <<: *RIGHT_POS + <<: *FRONT_POS - step: "ready get small island silver" arm: joints: [*JOINT1_READY_GET_SMALL_POS, *JOINT2_GET_SMALL_ISLAND_L_POS, *JOINT3_GET_SMALL_ISLAND_L_POS, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_SMALL1] @@ -487,6 +496,7 @@ steps_list: - step: "open main gripper" gripper: <<: *OPEN_MAIN_GRIPPER + GET_SMALL_ISLAND0: - step: "get small island silver" arm: joints: [ *JOINT1_GET_SMALL_POS, *JOINT2_GET_SMALL_ISLAND_L_POS, *JOINT3_GET_SMALL_ISLAND_L_POS, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_SMALL1 ] @@ -496,11 +506,22 @@ steps_list: <<: *NORMAL_TOLERANCE - step: "pull up silver" arm: - joints: [ *JOINT1_READY_GET_SMALL_POS, *JOINT2_GET_SMALL_ISLAND_L_POS, *JOINT3_GET_SMALL_ISLAND_L_POS, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_SMALL1 ] + joints: [ *JOINT1_READY_GET_STORED_POS, *JOINT2_GET_SMALL_ISLAND_L_POS, *JOINT3_GET_SMALL_ISLAND_L_POS, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_SMALL1 ] common: <<: *NORMALLY tolerance: <<: *NORMAL_TOLERANCE + - step: "chassis back" + chassis: + <<: *CHASSIS_MOVE_BACK + - step: "move stone away" + arm: + joints: [ *JOINT1_GET_GROUND_POS, 0.94, -1.266, *JOINT4_ZERO_POS, *JOINT5_DOWN_90 , 0.000001 ] + common: + <<: *NORMALLY + tolerance: + <<: *NORMAL_TOLERANCE + STORE_SILVER_TO_GRIPPER1: - step: "silver rotator up" @@ -804,7 +825,7 @@ steps_list: arm: joints: [ *JOINT1_READY_GET_STORED_POS, *JOINT2_GET_STORED_S3_POS, *JOINT3_GET_STORED_SILVER3_POS, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_GET_S3 ] common: - <<: *NORMALLY + <<: *SLOWLY tolerance: <<: *BIGGER_TOLERANCE - step: "left arm" @@ -1025,6 +1046,9 @@ steps_list: - step: "open gripper" gripper: <<: *OPEN_S3 + - step: "open silver gripper" + gripper: + <<: *OPEN_S_GRIPPER - step: "lift gimbal up" arm: joints: [ *JOINT1_GET_SMALL_POS, *JOINT2_AVOID_CONTROLLER_POS, *JOINT3_HOME_POS, *JOINT4_HOME_POS, *JOINT5_HOME_POS, *JOINT6_HOME_POS ] @@ -1035,15 +1059,6 @@ steps_list: - step: "gimbal look right" gimbal: <<: *RIGHT_POS - - step: "open gripper" - gripper: - <<: *OPEN_S1 - - step: "open gripper" - gripper: - <<: *OPEN_S2 - - step: "open gripper" - gripper: - <<: *OPEN_S3 - step: "rotate down" silver_rotator: target: 0.0001 @@ -1053,28 +1068,16 @@ steps_list: - step: "pusher back" silver_pusher: target: -0.00001 - - step: "turn 180" - chassis: - <<: *CHASSIS_TURN_180 - - step: "open gripper" - gripper: - <<: *OPEN_S1 - - step: "open gripper" - gripper: - <<: *OPEN_S2 - - step: "open gripper" - gripper: - <<: *OPEN_S3 - - step: "open silver gripper" - gripper: - <<: *OPEN_S_GRIPPER - step: "gimbal look back" gimbal: <<: *BACK_POS + - step: "turn 180" + chassis: + <<: *CHASSIS_TURN_180 GRIPPER_THREE_SILVER0: - step: "get silver" silver_pusher: - target: -0.04 + target: -0.06 delay: 0.5 - step: "pusher back little" silver_pusher: @@ -1086,8 +1089,8 @@ steps_list: delay: 0.5 - step: "pusher back" silver_pusher: - target: 0. - delay: 0.1 + target: 0.05 + delay: 0.05 - step: "rotate up" silver_rotator: target: 1.64 @@ -1139,7 +1142,7 @@ steps_list: - step: "get gold" gold_pusher: target: -0.443 - delay: 1.2 + delay: 2 - step: "gimbal look front" gimbal: <<: *FRONT_POS @@ -1393,6 +1396,13 @@ steps_list: middle_pitch: target: *MIDDLE_PITCH_NORMAL delay: 0.3 + - step: "lift arm" + arm: + joints: [ *JOINT1_READY_GET_STORED_POS, -1.2, -2.063, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ] + common: + <<: *QUICKLY + tolerance: + <<: *BIGGER_TOLERANCE - step: "ready to get silver from gripper1" arm: joints: [ *JOINT1_READY_GET_STORED_POS, *JOINT2_GET_STORED_S1_POS, *JOINT3_GET_STORED_SILVER1_POS, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_GET_S1 ] @@ -1421,12 +1431,19 @@ steps_list: arm: joints: [ *JOINT1_READY_GET_STORED_POS, *JOINT2_GET_STORED_S1_POS, *JOINT3_GET_STORED_SILVER1_POS, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_GET_S1 ] common: - <<: *NORMALLY + <<: *QUICKLY tolerance: <<: *NORMAL_TOLERANCE - step: "close silver gripper" gripper: <<: *CLOSE_S_GRIPPER + - step: "lift arm" + arm: + joints: [ *JOINT1_READY_GET_STORED_POS, -1.7, -1.263, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ] + common: + <<: *QUICKLY + tolerance: + <<: *BIGGER_TOLERANCE - step: "arm enter exchange pos" arm: <<: *LV5_L_EXCHANGE @@ -1441,6 +1458,13 @@ steps_list: middle_pitch: target: *MIDDLE_PITCH_NORMAL delay: 0.3 + - step: "lift arm" + arm: + joints: [ *JOINT1_READY_GET_STORED_POS, -1.2, -2.063, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ] + common: + <<: *QUICKLY + tolerance: + <<: *BIGGER_TOLERANCE - step: "ready to get silver from gripper2" arm: joints: [ *JOINT1_READY_GET_STORED_POS, *JOINT2_GET_STORED_S2_POS, *JOINT3_GET_STORED_SILVER2_POS, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_GET_S2 ] @@ -1471,6 +1495,13 @@ steps_list: <<: *NORMALLY tolerance: <<: *NORMAL_TOLERANCE + - step: "lift arm" + arm: + joints: [ *JOINT1_READY_GET_STORED_POS, -1.7, -1.263, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ] + common: + <<: *QUICKLY + tolerance: + <<: *BIGGER_TOLERANCE - step: "arm enter exchange pos" arm: <<: *LV5_L_EXCHANGE @@ -1487,16 +1518,16 @@ steps_list: delay: 0.3 - step: "lift arm" arm: - joints: [ *JOINT1_READY_GET_STORED_POS, 0.113, -2.063, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ] + joints: [ *JOINT1_READY_GET_STORED_POS, -1.2, -2.063, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ] common: <<: *QUICKLY tolerance: - <<: *NORMAL_TOLERANCE + <<: *BIGGER_TOLERANCE - step: "ready to get silver from gripper3" arm: joints: [ *JOINT1_READY_GET_STORED_POS, *JOINT2_GET_STORED_S3_POS, *JOINT3_GET_STORED_SILVER3_POS, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_GET_S3 ] common: - <<: *NORMALLY + <<: *QUICKLY tolerance: <<: *NORMAL_TOLERANCE - step: "open main gripper" @@ -1519,7 +1550,14 @@ steps_list: arm: joints: [ *JOINT1_READY_GET_STORED_POS, *JOINT2_GET_STORED_S3_POS, *JOINT3_GET_STORED_SILVER3_POS, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_GET_S3 ] common: - <<: *NORMALLY + <<: *QUICKLY + tolerance: + <<: *BIGGER_TOLERANCE + - step: "lift arm" + arm: + joints: [ *JOINT1_READY_GET_STORED_POS, -1.7, -1.263, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ] + common: + <<: *QUICKLY tolerance: <<: *BIGGER_TOLERANCE - step: "arm enter exchange pos" @@ -1535,7 +1573,7 @@ steps_list: delay: 0.3 - step: "ready to get stored gold" arm: - joints: [ *JOINT1_GET_STORED_GOLD_POS, *JOINT2_GET_STORED_GOLD_POS, *JOINT3_READY_GET_STORED_GOLD_POS, *JOINT4_R90_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ] + joints: [ *JOINT1_GET_STORED_GOLD_POS, *JOINT2_GET_STORED_GOLD_POS, *JOINT3_READY_GET_STORED_GOLD_POS, *JOINT4_R90_POS, *JOINT5_DOWN_90, *JOINT6_L_90 ] common: <<: *QUICKLY tolerance: @@ -1545,7 +1583,7 @@ steps_list: <<: *OPEN_MAIN_GRIPPER - step: "get stored gold" arm: - joints: [ *JOINT1_GET_STORED_GOLD_POS, *JOINT2_GET_STORED_GOLD_POS, *JOINT3_GET_STORED_GOLD_POS, *JOINT4_R90_POS, *JOINT5_GET_STORED_GOLD_POS, *JOINT6_HOME_POS ] + joints: [ *JOINT1_GET_STORED_GOLD_POS, *JOINT2_GET_STORED_GOLD_POS, *JOINT3_GET_STORED_GOLD_POS, *JOINT4_R90_POS, *JOINT5_GET_STORED_GOLD_POS, *JOINT6_L_90 ] common: <<: *NORMALLY tolerance: @@ -1561,14 +1599,14 @@ steps_list: change: "-g" - step: "move out gold" arm: - joints: [ *JOINT1_GET_STORED_GOLD_POS, *JOINT2_GET_STORED_GOLD_POS, *JOINT3_READY_GET_STORED_GOLD_POS, *JOINT4_R90_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ] + joints: [ *JOINT1_GET_STORED_GOLD_POS, *JOINT2_GET_STORED_GOLD_POS, *JOINT3_READY_GET_STORED_GOLD_POS, *JOINT4_R90_POS, *JOINT5_DOWN_90, *JOINT6_L_90 ] common: <<: *NORMALLY tolerance: <<: *BIGGER_TOLERANCE - step: "lift up gold" arm: - joints: [ *JOINT1_EXCHANGE_POS, *JOINT2_GET_STORED_GOLD_POS, *JOINT3_READY_GET_STORED_GOLD_POS, *JOINT4_R90_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ] + joints: [ *JOINT1_EXCHANGE_POS, *JOINT2_GET_STORED_GOLD_POS, *JOINT3_READY_GET_STORED_GOLD_POS, *JOINT4_R90_POS, *JOINT5_DOWN_90, *JOINT6_L_90 ] common: <<: *NORMALLY tolerance: @@ -1584,6 +1622,13 @@ steps_list: middle_pitch: target: *MIDDLE_PITCH_NORMAL delay: 0.3 + - step: "lift arm" + arm: + joints: [ *JOINT1_READY_GET_STORED_POS, -1.2, -2.063, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ] + common: + <<: *QUICKLY + tolerance: + <<: *BIGGER_TOLERANCE - step: "ready to get silver from gripper1" arm: joints: [ *JOINT1_READY_GET_STORED_POS, *JOINT2_GET_STORED_S1_POS, *JOINT3_GET_STORED_SILVER1_POS, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_GET_S1 ] @@ -1612,12 +1657,19 @@ steps_list: arm: joints: [ *JOINT1_READY_GET_STORED_POS, *JOINT2_GET_STORED_S1_POS, *JOINT3_GET_STORED_SILVER1_POS, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_GET_S1 ] common: - <<: *NORMALLY + <<: *QUICKLY tolerance: <<: *NORMAL_TOLERANCE - step: "close silver gripper" gripper: <<: *CLOSE_S_GRIPPER + - step: "lift arm" + arm: + joints: [ *JOINT1_READY_GET_STORED_POS, -1.7, -1.263, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ] + common: + <<: *QUICKLY + tolerance: + <<: *BIGGER_TOLERANCE - step: "arm enter exchange pos" arm: <<: *LV5_R_EXCHANGE @@ -1629,6 +1681,13 @@ steps_list: middle_pitch: target: *MIDDLE_PITCH_NORMAL delay: 0.3 + - step: "lift arm" + arm: + joints: [ *JOINT1_READY_GET_STORED_POS, -1.2, -2.063, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ] + common: + <<: *QUICKLY + tolerance: + <<: *BIGGER_TOLERANCE - step: "ready to get silver from gripper2" arm: joints: [ *JOINT1_READY_GET_STORED_POS, *JOINT2_GET_STORED_S2_POS, *JOINT3_GET_STORED_SILVER2_POS, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_GET_S2 ] @@ -1659,6 +1718,13 @@ steps_list: <<: *NORMALLY tolerance: <<: *NORMAL_TOLERANCE + - step: "lift arm" + arm: + joints: [ *JOINT1_READY_GET_STORED_POS, -1.7, -1.263, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ] + common: + <<: *QUICKLY + tolerance: + <<: *BIGGER_TOLERANCE - step: "arm enter exchange pos" arm: <<: *LV5_R_EXCHANGE @@ -1672,11 +1738,11 @@ steps_list: delay: 0.3 - step: "lift arm" arm: - joints: [ *JOINT1_READY_GET_STORED_POS, 0.113, -2.063, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ] + joints: [ *JOINT1_READY_GET_STORED_POS, -1.2, -2.063, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ] common: <<: *QUICKLY tolerance: - <<: *NORMAL_TOLERANCE + <<: *BIGGER_TOLERANCE - step: "ready to get silver from gripper3" arm: joints: [ *JOINT1_READY_GET_STORED_POS, *JOINT2_GET_STORED_S3_POS, *JOINT3_GET_STORED_SILVER3_POS, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_GET_S3 ] @@ -1704,7 +1770,14 @@ steps_list: arm: joints: [ *JOINT1_READY_GET_STORED_POS, *JOINT2_GET_STORED_S3_POS, *JOINT3_GET_STORED_SILVER3_POS, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_GET_S3 ] common: - <<: *NORMALLY + <<: *QUICKLY + tolerance: + <<: *BIGGER_TOLERANCE + - step: "lift arm" + arm: + joints: [ *JOINT1_READY_GET_STORED_POS, -1.7, -1.263, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ] + common: + <<: *QUICKLY tolerance: <<: *BIGGER_TOLERANCE - step: "arm enter exchange pos" From 15273a073efeada4d5d327deade05b0940c5bccc Mon Sep 17 00:00:00 2001 From: cc0h <7921166012@qq.com> Date: Thu, 8 Aug 2024 16:06:47 +0800 Subject: [PATCH 76/80] Record. --- .../config/engineer2_steps_list.yaml | 370 ++++++++++++------ 1 file changed, 241 insertions(+), 129 deletions(-) diff --git a/engineer_middleware/config/engineer2_steps_list.yaml b/engineer_middleware/config/engineer2_steps_list.yaml index f280a96..ae3b0b5 100644 --- a/engineer_middleware/config/engineer2_steps_list.yaml +++ b/engineer_middleware/config/engineer2_steps_list.yaml @@ -5,12 +5,12 @@ common: accel: 0.1 timeout: 15. normally: &NORMALLY - speed: 0.6 - accel: 0.6 + speed: 0.4 + accel: 0.4 timeout: 6 quickly: &QUICKLY - speed: 1 - accel: 1 + speed: 0.6 + accel: 0.5 timeout: 6. tolerance: @@ -33,7 +33,7 @@ common: ready_get_small: &JOINT1_READY_GET_SMALL_POS 0.42 get_small: &JOINT1_GET_SMALL_POS - 0.34 + 0.325 ready_get_stored: &JOINT1_READY_GET_STORED_POS 0.54 get_stored: &JOINT1_GET_STORED_POS @@ -76,7 +76,9 @@ common: get_stored_silver1: &JOINT2_GET_STORED_S1_POS -3.833 get_stored_gold: &JOINT2_GET_STORED_GOLD_POS - -1.257 + -1.062 + zero_pos: &JOINTN2_ZERO_POS + 0.000001 joint3: home: &JOINT3_HOME_POS @@ -108,7 +110,7 @@ common: ready_get_stored_gold: &JOINT3_READY_GET_STORED_GOLD_POS -1.67 get_stored_gold: &JOINT3_GET_STORED_GOLD_POS - -1.75 + -1.868 joint4: home: &JOINT4_HOME_POS @@ -120,7 +122,7 @@ common: l90: &JOINT4_L90_POS 1.57 get_stored_gold: &JOINT4_GET_STORED_GOLD_POS - -1.721 + -1.627 joint5: @@ -131,7 +133,7 @@ common: down_90: &JOINT5_DOWN_90 1.57 get_stored_gold: &JOINT5_GET_STORED_GOLD_POS - 1.726 + 1.765 store_to_s1: &JOINT5_STORE_TO_S1_POS -0.215 store_to_s2: &JOINT5_STORE_TO_S2_POS @@ -196,6 +198,20 @@ common: tolerance: <<: *NORMAL_TOLERANCE + lv5_r_straight_exchange: &LV5_R_STRAIGHT_EXCHANGE + joints: [ *JOINT1_EXCHANGE_POS, *JOINT2_EXCHANGE_POS, *JOINT3_ZERO_POS, *JOINT4_L90_POS, *JOINT5_DOWN_90, *JOINT6_R_90 ] + common: + <<: *NORMALLY + tolerance: + <<: *NORMAL_TOLERANCE + + lv5_l_straight_exchange: &LV5_L_STRAIGHT_EXCHANGE + joints: [ *JOINT1_EXCHANGE_POS, *JOINT2_EXCHANGE_POS, *JOINT3_ZERO_POS, *JOINT4_R90_POS, *JOINT5_DOWN_90, *JOINT6_L_90 ] + common: + <<: *NORMALLY + tolerance: + <<: *NORMAL_TOLERANCE + gimbal: right_pos: &RIGHT_POS frame: base_link @@ -212,43 +228,9 @@ common: follow_ee: &FOLLOW_EE frame: link6 position: [ 0.15, 0.00, 0. ] - - chassis_forward: &CHASSIS_FORWARD - frame: base_link - position: [ 0.1, 0. ] - yaw: 0.0 - chassis_tolerance_position: 0.1 - chassis_tolerance_angular: 0.2 - common: - timeout: 2. - - chassis_turn_180: &CHASSIS_TURN_180 - frame: base_link - position: [ 0., 0. ] - yaw: 1.57 - chassis_tolerance_position: 0.1 - chassis_tolerance_angular: 0.3 - common: - timeout: 4. - - chassis_fmove_left: &CHASSIS_MOVE_LEFT - frame: base_link - position: [ 0.0, 0.4 ] - yaw: 0.0 - chassis_tolerance_position: 0.1 - chassis_tolerance_angular: 0.2 - common: - timeout: 3. - - chassis_move_back: &CHASSIS_MOVE_BACK - frame: base_link - position: [ -0.4, 0.0 ] - yaw: 0.0 - chassis_tolerance_position: 0.1 - chassis_tolerance_angular: 0.2 - common: - timeout: 3. - + follow_gold_gripper: &FOLLOW_GOLD + frame: gold_lifter_link + position: [ 0.0, 0.0, 0.0 ] gripper: open_main_gripper: &OPEN_MAIN_GRIPPER @@ -324,6 +306,9 @@ steps_list: - step: "lifter down" silver_lifter: target: 0.0 + - step: "rotate up" + silver_rotator: + target: 1.64 - step: "get gold" gold_pusher: target: 0. @@ -376,24 +361,9 @@ steps_list: <<: *CLOSE_MAIN_GRIPPER HOME_WITH_PITCH: - - step: "main close" - gripper: - <<: *CLOSE_MAIN_GRIPPER - - step: "s1 close" - gripper: - <<: *CLOSE_S1 - - step: "s2 close" - gripper: - <<: *CLOSE_S2 - - step: "s3 close" - gripper: - <<: *CLOSE_S3 - - step: "g1 close" - gripper: - <<: *CLOSE_GOLD - - step: "close silver gripper" - gripper: - <<: *CLOSE_S_GRIPPER + - step: "rotate up" + silver_rotator: + target: 1.64 - step: "gimbal look front" gimbal: <<: *FRONT_POS @@ -422,7 +392,7 @@ steps_list: - step: "middle pitch up" middle_pitch: target: *MIDDLE_PITCH_NORMAL - delay: 0.3 + delay: 0.6 - step: "arm enter exchange pos" arm: <<: *LV5_L_EXCHANGE @@ -434,7 +404,7 @@ steps_list: - step: "middle pitch up" middle_pitch: target: *MIDDLE_PITCH_NORMAL - delay: 0.3 + delay: 0.6 - step: "arm enter exchange pos" arm: <<: *LV5_R_EXCHANGE @@ -442,6 +412,30 @@ steps_list: gimbal: <<: *RIGHT_POS + LV5_L_STRAIGHT_EXCHANGE: + - step: "middle pitch up" + middle_pitch: + target: *MIDDLE_PITCH_NORMAL + delay: 0.3 + - step: "arm enter exchange pos" + arm: + <<: *LV5_L_STRAIGHT_EXCHANGE + - step: "gimbal ready" + gimbal: + <<: *RIGHT_POS + + LV5_R_STRAIGHT_EXCHANGE: + - step: "middle pitch up" + middle_pitch: + target: *MIDDLE_PITCH_NORMAL + delay: 0.3 + - step: "arm enter exchange pos" + arm: + <<: *LV5_R_STRAIGHT_EXCHANGE + - step: "gimbal ready" + gimbal: + <<: *RIGHT_POS + LV5_R_DROP_GOLD_EXCHANGE: - step: "middle pitch up" middle_pitch: @@ -511,9 +505,7 @@ steps_list: <<: *NORMALLY tolerance: <<: *NORMAL_TOLERANCE - - step: "chassis back" - chassis: - <<: *CHASSIS_MOVE_BACK + GET_SMALL_ISLAND00: - step: "move stone away" arm: joints: [ *JOINT1_GET_GROUND_POS, 0.94, -1.266, *JOINT4_ZERO_POS, *JOINT5_DOWN_90 , 0.000001 ] @@ -1053,7 +1045,7 @@ steps_list: arm: joints: [ *JOINT1_GET_SMALL_POS, *JOINT2_AVOID_CONTROLLER_POS, *JOINT3_HOME_POS, *JOINT4_HOME_POS, *JOINT5_HOME_POS, *JOINT6_HOME_POS ] common: - <<: *QUICKLY + <<: *NORMALLY tolerance: <<: *BIGGER_TOLERANCE - step: "gimbal look right" @@ -1064,37 +1056,40 @@ steps_list: target: 0.0001 - step: "lifter ready" silver_lifter: - target: 0.07 + target: 0.075 - step: "pusher back" silver_pusher: target: -0.00001 - step: "gimbal look back" gimbal: <<: *BACK_POS - - step: "turn 180" - chassis: - <<: *CHASSIS_TURN_180 GRIPPER_THREE_SILVER0: - step: "get silver" silver_pusher: - target: -0.06 - delay: 0.5 + target: -0.042 + GRIPPER_THREE_SILVER00: + - step: "lifter ready" + silver_lifter: + target: 0.08 - step: "pusher back little" silver_pusher: - target: -0.015 - delay: 0.2 + target: -0.02 + delay: 0.5 + - step: "rotate up" + silver_rotator: + target: 0.1 - step: "lifter up" silver_lifter: target: 0.2 delay: 0.5 - step: "pusher back" silver_pusher: - target: 0.05 - delay: 0.05 + target: 0.0001 + GRIPPER_THREE_SILVER000: - step: "rotate up" silver_rotator: target: 1.64 - delay: 0.5 + delay: 0.4 - step: "lifter down" silver_lifter: target: 0.05 @@ -1141,15 +1136,41 @@ steps_list: MID_BIG_ISLAND0: - step: "get gold" gold_pusher: - target: -0.443 - delay: 2 + target: -0.5 + MID_BIG_ISLAND00: + - step: "pull gold back" + gold_pusher: + target: -0.444 + delay: 0.2 + - step: "lift gold" + gold_lifter: + target: 0.062 + delay: 0.6 + MID_BIG_ISLAND000: + - step: "pull back gold" + gold_pusher: + target: -0.054 + delay: 1 + - step: "add stone" + stone_num: + change: "+g" - step: "gimbal look front" gimbal: <<: *FRONT_POS + + MID_BIG_ISLAND1: + - step: "get gold" + gold_pusher: + target: -0.5 + delay: 2.0 + - step: "pull gold back" + gold_pusher: + target: -0.444 + delay: 0.1 - step: "lift gold" gold_lifter: - target: 0.06 - delay: 0.5 + target: 0.062 + delay: 0.6 - step: "pull back gold" gold_pusher: target: -0.054 @@ -1157,9 +1178,9 @@ steps_list: - step: "add stone" stone_num: change: "+g" - - step: "chassis left" - chassis: - <<: *CHASSIS_MOVE_LEFT + - step: "gimbal look front" + gimbal: + <<: *FRONT_POS @@ -1173,14 +1194,41 @@ steps_list: delay: 0.3 - step: "arm ready" arm: - joints: [ 0.040,0.885,-2.107,*JOINT4_R90_POS,-0.00001, *JOINT6_HOME_POS ] + joints: [ 0.040,0.885,-2.107,*JOINT4_R90_POS,-0.00001, *JOINT6_L_90 ] common: - <<: *QUICKLY + <<: *NORMALLY tolerance: <<: *NORMAL_TOLERANCE - step: "open main gripper" gripper: <<: *OPEN_MAIN_GRIPPER + SIDE_BIG_ISLAND1: + - step: "gimbal look ee" + gimbal: + <<: *FOLLOW_EE + - step: "arm get gold" + arm: + joints: [ 0.040,0.147,-1.429,*JOINT4_R90_POS, 0.08, *JOINT6_L_90 ] + common: + <<: *NORMALLY + tolerance: + <<: *BIGGER_TOLERANCE + SIDE_BIG_ISLAND11: + - step: "gimbal look right" + gimbal: + <<: *RIGHT_POS + SIDE_BIG_ISLAND111: + - step: "move stone towards down" + arm: + joints: [ 0.127, 0.997, -1.163, 0.0001, 1.57, -1.57 ] + common: + <<: *NORMALLY + tolerance: + <<: *NORMAL_TOLERANCE + - step: "gimbal look front" + gimbal: + <<: *FRONT_POS + SIDE_BIG_ISLAND0: - step: "arm get gold" arm: @@ -1206,9 +1254,6 @@ steps_list: <<: *SLOWLY tolerance: <<: *BIGGER_TOLERANCE - - step: "chassis left" - chassis: - <<: *CHASSIS_MOVE_LEFT - step: "move stone towards down" arm: joints: [ 0.127, 0.997, -1.163, 0.0001, 1.57, -1.57 ] @@ -1246,18 +1291,19 @@ steps_list: delay: 0.3 - step: "arm ready" arm: - joints: [ 0.040,0.885,-2.107,*JOINT4_R90_POS,-0.00001, *JOINT6_HOME_POS ] + joints: [ 0.040,0.885,-2.107,*JOINT4_R90_POS,-0.00001, *JOINT6_L_90 ] common: - <<: *QUICKLY + <<: *NORMALLY tolerance: <<: *NORMAL_TOLERANCE - step: "open main gripper" gripper: <<: *OPEN_MAIN_GRIPPER + BOTH_BIG_ISLAND0: - step: "get gold" gold_pusher: - target: -0.443 + target: -0.5 - step: "arm get gold" arm: joints: [ 0.040,0.147,-1.429,*JOINT4_R90_POS, 0.08, *JOINT6_HOME_POS ] @@ -1265,9 +1311,17 @@ steps_list: <<: *NORMALLY tolerance: <<: *BIGGER_TOLERANCE + BOTH_BIG_ISLAND00: + - step: "gimbal look right" + gimbal: + <<: *RIGHT_POS + - step: "pull gold" + gold_pusher: + target: -0.44 + delay: 0.3 - step: "lift gold" gold_lifter: - target: 0.055 + target: 0.062 - step: "arm lift gold" arm: joints: [ 0.114,0.147,-1.429,*JOINT4_R90_POS, 0.08, *JOINT6_HOME_POS ] @@ -1277,23 +1331,67 @@ steps_list: <<: *NORMAL_TOLERANCE - step: "pull back gold" gold_pusher: - target: -0.054 + target: -0.09 - step: "add stone" stone_num: change: "+g" - - step: "gimbal look front" - gimbal: - <<: *FRONT_POS - step: "arm pull back gold" arm: - joints: [ 0.114,0.988,-1.927, *JOINT4_R90_POS,0.344, *JOINT6_HOME_POS ] + joints: [ 0.104,0.988,-1.927, *JOINT4_R90_POS,0.344, *JOINT6_HOME_POS ] common: <<: *SLOWLY tolerance: <<: *BIGGER_TOLERANCE - - step: "chassis left" - chassis: - <<: *CHASSIS_MOVE_LEFT + BOTH_BIG_ISLAND000: + - step: "gimbal look right" + gimbal: + <<: *FRONT_POS + - step: "pull back gold" + gold_pusher: + target: -0.054 + - step: "move stone towards down" + arm: + joints: [ 0.127, 0.997, -1.163, 0.0001, 1.57, -1.57 ] + common: + <<: *NORMALLY + tolerance: + <<: *NORMAL_TOLERANCE + + BOTH_BIG_ISLAND1: + - step: "get gold" + gold_pusher: + target: -0.5 + - step: "arm get gold" + arm: + joints: [ 0.040,0.147,-1.429,*JOINT4_R90_POS, 0.08, *JOINT6_L_90 ] + common: + <<: *NORMALLY + tolerance: + <<: *BIGGER_TOLERANCE + - step: "pull gold" + gold_pusher: + target: -0.444 + delay: 0.2 + - step: "lift gold" + gold_lifter: + target: 0.062 + - step: "gimbal look ee" + gimbal: + <<: *FOLLOW_EE + BOTH_BIG_ISLAND11: + - step: "gimbal look right" + gimbal: + <<: *RIGHT_POS + BOTH_BIG_ISLAND111: + - step: "pull back gold" + gold_pusher: + target: -0.054 + - step: "add stone" + stone_num: + change: "+g" + - step: "gimbal look front" + gimbal: + <<: *FRONT_POS - step: "move stone towards down" arm: joints: [ 0.127, 0.997, -1.163, 0.0001, 1.57, -1.57 ] @@ -1400,14 +1498,14 @@ steps_list: arm: joints: [ *JOINT1_READY_GET_STORED_POS, -1.2, -2.063, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ] common: - <<: *QUICKLY + <<: *NORMALLY tolerance: <<: *BIGGER_TOLERANCE - step: "ready to get silver from gripper1" arm: joints: [ *JOINT1_READY_GET_STORED_POS, *JOINT2_GET_STORED_S1_POS, *JOINT3_GET_STORED_SILVER1_POS, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_GET_S1 ] common: - <<: *QUICKLY + <<: *NORMALLY tolerance: <<: *NORMAL_TOLERANCE @@ -1431,7 +1529,7 @@ steps_list: arm: joints: [ *JOINT1_READY_GET_STORED_POS, *JOINT2_GET_STORED_S1_POS, *JOINT3_GET_STORED_SILVER1_POS, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_GET_S1 ] common: - <<: *QUICKLY + <<: *NORMALLY tolerance: <<: *NORMAL_TOLERANCE - step: "close silver gripper" @@ -1441,9 +1539,12 @@ steps_list: arm: joints: [ *JOINT1_READY_GET_STORED_POS, -1.7, -1.263, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ] common: - <<: *QUICKLY + <<: *NORMALLY tolerance: <<: *BIGGER_TOLERANCE + - step: "rotate down" + silver_rotator: + target: 0.0001 - step: "arm enter exchange pos" arm: <<: *LV5_L_EXCHANGE @@ -1462,14 +1563,14 @@ steps_list: arm: joints: [ *JOINT1_READY_GET_STORED_POS, -1.2, -2.063, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ] common: - <<: *QUICKLY + <<: *NORMALLY tolerance: <<: *BIGGER_TOLERANCE - step: "ready to get silver from gripper2" arm: joints: [ *JOINT1_READY_GET_STORED_POS, *JOINT2_GET_STORED_S2_POS, *JOINT3_GET_STORED_SILVER2_POS, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_GET_S2 ] common: - <<: *QUICKLY + <<: *NORMALLY tolerance: <<: *NORMAL_TOLERANCE - step: "open main gripper" @@ -1499,7 +1600,7 @@ steps_list: arm: joints: [ *JOINT1_READY_GET_STORED_POS, -1.7, -1.263, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ] common: - <<: *QUICKLY + <<: *NORMALLY tolerance: <<: *BIGGER_TOLERANCE - step: "arm enter exchange pos" @@ -1520,14 +1621,14 @@ steps_list: arm: joints: [ *JOINT1_READY_GET_STORED_POS, -1.2, -2.063, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ] common: - <<: *QUICKLY + <<: *NORMALLY tolerance: <<: *BIGGER_TOLERANCE - step: "ready to get silver from gripper3" arm: joints: [ *JOINT1_READY_GET_STORED_POS, *JOINT2_GET_STORED_S3_POS, *JOINT3_GET_STORED_SILVER3_POS, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_GET_S3 ] common: - <<: *QUICKLY + <<: *NORMALLY tolerance: <<: *NORMAL_TOLERANCE - step: "open main gripper" @@ -1550,14 +1651,14 @@ steps_list: arm: joints: [ *JOINT1_READY_GET_STORED_POS, *JOINT2_GET_STORED_S3_POS, *JOINT3_GET_STORED_SILVER3_POS, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_GET_S3 ] common: - <<: *QUICKLY + <<: *NORMALLY tolerance: <<: *BIGGER_TOLERANCE - step: "lift arm" arm: joints: [ *JOINT1_READY_GET_STORED_POS, -1.7, -1.263, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ] common: - <<: *QUICKLY + <<: *NORMALLY tolerance: <<: *BIGGER_TOLERANCE - step: "arm enter exchange pos" @@ -1567,6 +1668,9 @@ steps_list: gimbal: <<: *RIGHT_POS LV5_L_G: + - step: "rotate down" + silver_rotator: + target: 0.000001 - step: "middle pitch up" middle_pitch: target: *MIDDLE_PITCH_NORMAL @@ -1575,7 +1679,7 @@ steps_list: arm: joints: [ *JOINT1_GET_STORED_GOLD_POS, *JOINT2_GET_STORED_GOLD_POS, *JOINT3_READY_GET_STORED_GOLD_POS, *JOINT4_R90_POS, *JOINT5_DOWN_90, *JOINT6_L_90 ] common: - <<: *QUICKLY + <<: *NORMALLY tolerance: <<: *NORMAL_TOLERANCE - step: "open main gripper" @@ -1594,12 +1698,13 @@ steps_list: - step: "move gold gripper back" gold_pusher: target: 0.00001 + delay: 0.3 - step: "add stone" stone_num: change: "-g" - step: "move out gold" arm: - joints: [ *JOINT1_GET_STORED_GOLD_POS, *JOINT2_GET_STORED_GOLD_POS, *JOINT3_READY_GET_STORED_GOLD_POS, *JOINT4_R90_POS, *JOINT5_DOWN_90, *JOINT6_L_90 ] + joints: [ *JOINT1_GET_STORED_GOLD_POS, *JOINT2_GET_STORED_GOLD_POS, *JOINT3_GET_STORED_GOLD_POS, *JOINT4_R90_POS, *JOINT5_GET_STORED_GOLD_POS, *JOINT6_L_90 ] common: <<: *NORMALLY tolerance: @@ -1626,14 +1731,14 @@ steps_list: arm: joints: [ *JOINT1_READY_GET_STORED_POS, -1.2, -2.063, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ] common: - <<: *QUICKLY + <<: *NORMALLY tolerance: <<: *BIGGER_TOLERANCE - step: "ready to get silver from gripper1" arm: joints: [ *JOINT1_READY_GET_STORED_POS, *JOINT2_GET_STORED_S1_POS, *JOINT3_GET_STORED_SILVER1_POS, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_GET_S1 ] common: - <<: *QUICKLY + <<: *NORMALLY tolerance: <<: *NORMAL_TOLERANCE @@ -1657,7 +1762,7 @@ steps_list: arm: joints: [ *JOINT1_READY_GET_STORED_POS, *JOINT2_GET_STORED_S1_POS, *JOINT3_GET_STORED_SILVER1_POS, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_GET_S1 ] common: - <<: *QUICKLY + <<: *NORMALLY tolerance: <<: *NORMAL_TOLERANCE - step: "close silver gripper" @@ -1667,9 +1772,12 @@ steps_list: arm: joints: [ *JOINT1_READY_GET_STORED_POS, -1.7, -1.263, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ] common: - <<: *QUICKLY + <<: *NORMALLY tolerance: <<: *BIGGER_TOLERANCE + - step: "rotate down" + silver_rotator: + target: 0.0001 - step: "arm enter exchange pos" arm: <<: *LV5_R_EXCHANGE @@ -1685,14 +1793,14 @@ steps_list: arm: joints: [ *JOINT1_READY_GET_STORED_POS, -1.2, -2.063, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ] common: - <<: *QUICKLY + <<: *NORMALLY tolerance: <<: *BIGGER_TOLERANCE - step: "ready to get silver from gripper2" arm: joints: [ *JOINT1_READY_GET_STORED_POS, *JOINT2_GET_STORED_S2_POS, *JOINT3_GET_STORED_SILVER2_POS, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_GET_S2 ] common: - <<: *QUICKLY + <<: *NORMALLY tolerance: <<: *NORMAL_TOLERANCE - step: "open main gripper" @@ -1722,7 +1830,7 @@ steps_list: arm: joints: [ *JOINT1_READY_GET_STORED_POS, -1.7, -1.263, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ] common: - <<: *QUICKLY + <<: *NORMALLY tolerance: <<: *BIGGER_TOLERANCE - step: "arm enter exchange pos" @@ -1740,7 +1848,7 @@ steps_list: arm: joints: [ *JOINT1_READY_GET_STORED_POS, -1.2, -2.063, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ] common: - <<: *QUICKLY + <<: *NORMALLY tolerance: <<: *BIGGER_TOLERANCE - step: "ready to get silver from gripper3" @@ -1770,14 +1878,14 @@ steps_list: arm: joints: [ *JOINT1_READY_GET_STORED_POS, *JOINT2_GET_STORED_S3_POS, *JOINT3_GET_STORED_SILVER3_POS, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_GET_S3 ] common: - <<: *QUICKLY + <<: *NORMALLY tolerance: <<: *BIGGER_TOLERANCE - step: "lift arm" arm: joints: [ *JOINT1_READY_GET_STORED_POS, -1.7, -1.263, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ] common: - <<: *QUICKLY + <<: *NORMALLY tolerance: <<: *BIGGER_TOLERANCE - step: "arm enter exchange pos" @@ -1787,6 +1895,9 @@ steps_list: gimbal: <<: *RIGHT_POS LV5_R_G: + - step: "rotate down" + silver_rotator: + target: 0.000001 - step: "middle pitch up" middle_pitch: target: *MIDDLE_PITCH_NORMAL @@ -1814,12 +1925,13 @@ steps_list: - step: "move gold gripper back" gold_pusher: target: 0.00001 + delay: 0.3 - step: "add stone" stone_num: change: "-g" - step: "move out gold" arm: - joints: [ *JOINT1_GET_STORED_GOLD_POS, *JOINT2_GET_STORED_GOLD_POS, *JOINT3_READY_GET_STORED_GOLD_POS, *JOINT4_R90_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ] + joints: [ *JOINT1_GET_STORED_GOLD_POS, *JOINT2_GET_STORED_GOLD_POS, *JOINT3_GET_STORED_GOLD_POS, *JOINT4_R90_POS, *JOINT5_GET_STORED_GOLD_POS, *JOINT6_HOME_POS ] common: <<: *NORMALLY tolerance: From 3fe3719b3b97a16c454cf6599ed242a314391acd Mon Sep 17 00:00:00 2001 From: cc0h <7921166012@qq.com> Date: Thu, 8 Aug 2024 17:33:16 +0800 Subject: [PATCH 77/80] Improve gripper 3 silver. --- engineer_middleware/config/engineer2_steps_list.yaml | 12 +++++------- 1 file changed, 5 insertions(+), 7 deletions(-) diff --git a/engineer_middleware/config/engineer2_steps_list.yaml b/engineer_middleware/config/engineer2_steps_list.yaml index ae3b0b5..fec9248 100644 --- a/engineer_middleware/config/engineer2_steps_list.yaml +++ b/engineer_middleware/config/engineer2_steps_list.yaml @@ -1082,10 +1082,10 @@ steps_list: silver_lifter: target: 0.2 delay: 0.5 + GRIPPER_THREE_SILVER000: - step: "pusher back" silver_pusher: target: 0.0001 - GRIPPER_THREE_SILVER000: - step: "rotate up" silver_rotator: target: 1.64 @@ -1093,7 +1093,7 @@ steps_list: - step: "lifter down" silver_lifter: target: 0.05 - delay: 0.15 + delay: 0.3 - step: "lifter down" silver_lifter: target: 0.0 @@ -1136,16 +1136,14 @@ steps_list: MID_BIG_ISLAND0: - step: "get gold" gold_pusher: - target: -0.5 + target: -0.438 MID_BIG_ISLAND00: - step: "pull gold back" gold_pusher: - target: -0.444 - delay: 0.2 + target: -0.42 - step: "lift gold" gold_lifter: - target: 0.062 - delay: 0.6 + target: 0.055 MID_BIG_ISLAND000: - step: "pull back gold" gold_pusher: From 570501d5b86c56371b9e2aca311e43f79d878e3d Mon Sep 17 00:00:00 2001 From: cc0h <7921166012@qq.com> Date: Fri, 9 Aug 2024 10:23:49 +0800 Subject: [PATCH 78/80] Add improved direction change motion, place stone on arm to a stable place. --- .../config/engineer2_steps_list.yaml | 196 ++++++++++++++---- 1 file changed, 160 insertions(+), 36 deletions(-) diff --git a/engineer_middleware/config/engineer2_steps_list.yaml b/engineer_middleware/config/engineer2_steps_list.yaml index fec9248..6521b06 100644 --- a/engineer_middleware/config/engineer2_steps_list.yaml +++ b/engineer_middleware/config/engineer2_steps_list.yaml @@ -6,7 +6,7 @@ common: timeout: 15. normally: &NORMALLY speed: 0.4 - accel: 0.4 + accel: 0.3 timeout: 6 quickly: &QUICKLY speed: 0.6 @@ -392,7 +392,7 @@ steps_list: - step: "middle pitch up" middle_pitch: target: *MIDDLE_PITCH_NORMAL - delay: 0.6 + delay: 1.0 - step: "arm enter exchange pos" arm: <<: *LV5_L_EXCHANGE @@ -400,11 +400,64 @@ steps_list: gimbal: <<: *RIGHT_POS + L2R_EXCHANGE: + - step: "arm move back" + arm: + joints: [*JOINT1_EXCHANGE_POS, 0.92, -2.38, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS] + common: + <<: *NORMALLY + tolerance: + <<: *BIGGER_TOLERANCE + - step: "reverse joint2 and joint3" + arm: + joints: [*JOINT1_EXCHANGE_POS, -0.846, 2.20, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS] + common: + <<: *NORMALLY + tolerance: + <<: *BIGGER_TOLERANCE + - step: "rotate joint2 to r exchange pos" + arm: + joints: [*JOINT1_EXCHANGE_POS, *JOINT2_R_EXCHANGE_POS, 2.20, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS] + common: + <<: *NORMALLY + tolerance: + <<: *BIGGER_TOLERANCE + - step: "arm enter exchange pos" + arm: + <<: *LV5_R_EXCHANGE + + R2L_EXCHANGE: + - step: "fold arm" + arm: + joints: [ *JOINT1_EXCHANGE_POS, *JOINT2_R_EXCHANGE_POS, 2.20, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ] + common: + <<: *NORMALLY + tolerance: + <<: *BIGGER_TOLERANCE + - step: "rotate joint2 to l exchange pos" + arm: + joints: [ *JOINT1_EXCHANGE_POS, -0.846, 2.20, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ] + common: + <<: *NORMALLY + tolerance: + <<: *BIGGER_TOLERANCE + - step: "reverse joint2 and joint3" + arm: + joints: [ *JOINT1_EXCHANGE_POS, 0.92, -2.38, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ] + common: + <<: *NORMALLY + tolerance: + <<: *BIGGER_TOLERANCE + - step: "arm enter exchange pos" + arm: + <<: *LV5_L_EXCHANGE + + LV5_R_EXCHANGE: - step: "middle pitch up" middle_pitch: target: *MIDDLE_PITCH_NORMAL - delay: 0.6 + delay: 1.0 - step: "arm enter exchange pos" arm: <<: *LV5_R_EXCHANGE @@ -506,13 +559,24 @@ steps_list: tolerance: <<: *NORMAL_TOLERANCE GET_SMALL_ISLAND00: - - step: "move stone away" + - step: "ready stuck stone" + arm: + joints: [ 0.24, 1.049, -2.4010, -0.225, 1.112, -0.11 ] + common: + <<: *NORMALLY + tolerance: + <<: *NORMAL_TOLERANCE + - step: "stuck stone" arm: - joints: [ *JOINT1_GET_GROUND_POS, 0.94, -1.266, *JOINT4_ZERO_POS, *JOINT5_DOWN_90 , 0.000001 ] + joints: [ 0.16, 1.049, -2.4010, -0.225, 1.112, -0.11 ] common: <<: *NORMALLY tolerance: <<: *NORMAL_TOLERANCE + - step: "get ground stone" + middle_pitch: + target: 0.49 + delay: 0.5 STORE_SILVER_TO_GRIPPER1: @@ -688,6 +752,23 @@ steps_list: <<: *NORMALLY tolerance: <<: *NORMAL_TOLERANCE + GROUND_STONE00: + - step: "ready stuck stone" + arm: + joints: [ 0.24, 1.049, -2.4010, -0.225, 1.112, -0.11] + common: + <<: *NORMALLY + tolerance: + <<: *NORMAL_TOLERANCE + - step: "stuck stone" + arm: + joints: [ 0.16, 1.049, -2.4010, -0.225, 1.112, -0.11] + common: + <<: *NORMALLY + tolerance: + <<: *NORMAL_TOLERANCE + + @@ -1136,19 +1217,18 @@ steps_list: MID_BIG_ISLAND0: - step: "get gold" gold_pusher: - target: -0.438 + target: -0.44 MID_BIG_ISLAND00: - step: "pull gold back" gold_pusher: target: -0.42 - step: "lift gold" gold_lifter: - target: 0.055 + target: 0.058 MID_BIG_ISLAND000: - step: "pull back gold" gold_pusher: target: -0.054 - delay: 1 - step: "add stone" stone_num: change: "+g" @@ -1159,20 +1239,18 @@ steps_list: MID_BIG_ISLAND1: - step: "get gold" gold_pusher: - target: -0.5 - delay: 2.0 + target: -0.44 + MID_BIG_ISLAND11: - step: "pull gold back" gold_pusher: - target: -0.444 - delay: 0.1 + target: -0.42 - step: "lift gold" gold_lifter: target: 0.062 - delay: 0.6 + delay: 0.7 - step: "pull back gold" gold_pusher: target: -0.054 - delay: 1 - step: "add stone" stone_num: change: "+g" @@ -1252,13 +1330,25 @@ steps_list: <<: *SLOWLY tolerance: <<: *BIGGER_TOLERANCE - - step: "move stone towards down" + SIDE_BIG_ISLAND00: + - step: "ready stuck stone" arm: - joints: [ 0.127, 0.997, -1.163, 0.0001, 1.57, -1.57 ] + joints: [ 0.24, 1.049, -2.4010, -0.225, 1.112, -0.11 ] common: <<: *NORMALLY tolerance: <<: *NORMAL_TOLERANCE + - step: "stuck stone" + arm: + joints: [ 0.16, 1.049, -2.4010, -0.225, 1.112, -0.11 ] + common: + <<: *NORMALLY + tolerance: + <<: *NORMAL_TOLERANCE + - step: "get ground stone" + middle_pitch: + target: 0.49 + delay: 0.5 BOTH_BIG_ISLAND: - step: "middle pitch up" @@ -1301,7 +1391,7 @@ steps_list: BOTH_BIG_ISLAND0: - step: "get gold" gold_pusher: - target: -0.5 + target: -0.44 - step: "arm get gold" arm: joints: [ 0.040,0.147,-1.429,*JOINT4_R90_POS, 0.08, *JOINT6_HOME_POS ] @@ -1315,11 +1405,10 @@ steps_list: <<: *RIGHT_POS - step: "pull gold" gold_pusher: - target: -0.44 - delay: 0.3 + target: -0.42 - step: "lift gold" gold_lifter: - target: 0.062 + target: 0.058 - step: "arm lift gold" arm: joints: [ 0.114,0.147,-1.429,*JOINT4_R90_POS, 0.08, *JOINT6_HOME_POS ] @@ -1327,9 +1416,10 @@ steps_list: <<: *NORMALLY tolerance: <<: *NORMAL_TOLERANCE + BOTH_BIG_ISLAND000: - step: "pull back gold" gold_pusher: - target: -0.09 + target: -0.054 - step: "add stone" stone_num: change: "+g" @@ -1340,20 +1430,33 @@ steps_list: <<: *SLOWLY tolerance: <<: *BIGGER_TOLERANCE - BOTH_BIG_ISLAND000: + BOTH_BIG_ISLAND0000: - step: "gimbal look right" gimbal: <<: *FRONT_POS - step: "pull back gold" gold_pusher: target: -0.054 - - step: "move stone towards down" + BOTH_BIG_ISLAND00000: + - step: "ready stuck stone" arm: - joints: [ 0.127, 0.997, -1.163, 0.0001, 1.57, -1.57 ] + joints: [ 0.24, 1.049, -2.4010, -0.225, 1.112, -0.11 ] common: <<: *NORMALLY tolerance: <<: *NORMAL_TOLERANCE + - step: "stuck stone" + arm: + joints: [ 0.16, 1.049, -2.4010, -0.225, 1.112, -0.11 ] + common: + <<: *NORMALLY + tolerance: + <<: *NORMAL_TOLERANCE + - step: "get ground stone" + middle_pitch: + target: 0.49 + delay: 0.5 + BOTH_BIG_ISLAND1: - step: "get gold" @@ -1491,7 +1594,7 @@ steps_list: - step: "middle pitch up" middle_pitch: target: *MIDDLE_PITCH_NORMAL - delay: 0.3 + delay: 0.6 - step: "lift arm" arm: joints: [ *JOINT1_READY_GET_STORED_POS, -1.2, -2.063, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ] @@ -1543,6 +1646,13 @@ steps_list: - step: "rotate down" silver_rotator: target: 0.0001 + - step: "arm move back" + arm: + joints: [ *JOINT1_EXCHANGE_POS, 0.92, -2.38, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ] + common: + <<: *NORMALLY + tolerance: + <<: *NORMAL_TOLERANCE - step: "arm enter exchange pos" arm: <<: *LV5_L_EXCHANGE @@ -1556,7 +1666,7 @@ steps_list: - step: "middle pitch up" middle_pitch: target: *MIDDLE_PITCH_NORMAL - delay: 0.3 + delay: 0.6 - step: "lift arm" arm: joints: [ *JOINT1_READY_GET_STORED_POS, -1.2, -2.063, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ] @@ -1601,6 +1711,13 @@ steps_list: <<: *NORMALLY tolerance: <<: *BIGGER_TOLERANCE + - step: "arm move back" + arm: + joints: [ *JOINT1_EXCHANGE_POS, 0.92, -2.38, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ] + common: + <<: *NORMALLY + tolerance: + <<: *NORMAL_TOLERANCE - step: "arm enter exchange pos" arm: <<: *LV5_L_EXCHANGE @@ -1614,7 +1731,7 @@ steps_list: - step: "middle pitch up" middle_pitch: target: *MIDDLE_PITCH_NORMAL - delay: 0.3 + delay: 0.6 - step: "lift arm" arm: joints: [ *JOINT1_READY_GET_STORED_POS, -1.2, -2.063, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ] @@ -1659,6 +1776,13 @@ steps_list: <<: *NORMALLY tolerance: <<: *BIGGER_TOLERANCE + - step: "arm move back" + arm: + joints: [ *JOINT1_EXCHANGE_POS, 0.92, -2.38, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ] + common: + <<: *NORMALLY + tolerance: + <<: *NORMAL_TOLERANCE - step: "arm enter exchange pos" arm: <<: *LV5_L_EXCHANGE @@ -1672,7 +1796,7 @@ steps_list: - step: "middle pitch up" middle_pitch: target: *MIDDLE_PITCH_NORMAL - delay: 0.3 + delay: 0.6 - step: "ready to get stored gold" arm: joints: [ *JOINT1_GET_STORED_GOLD_POS, *JOINT2_GET_STORED_GOLD_POS, *JOINT3_READY_GET_STORED_GOLD_POS, *JOINT4_R90_POS, *JOINT5_DOWN_90, *JOINT6_L_90 ] @@ -1724,7 +1848,7 @@ steps_list: - step: "middle pitch up" middle_pitch: target: *MIDDLE_PITCH_NORMAL - delay: 0.3 + delay: 0.6 - step: "lift arm" arm: joints: [ *JOINT1_READY_GET_STORED_POS, -1.2, -2.063, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ] @@ -1786,7 +1910,7 @@ steps_list: - step: "middle pitch up" middle_pitch: target: *MIDDLE_PITCH_NORMAL - delay: 0.3 + delay: 0.6 - step: "lift arm" arm: joints: [ *JOINT1_READY_GET_STORED_POS, -1.2, -2.063, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ] @@ -1841,7 +1965,7 @@ steps_list: - step: "middle pitch up" middle_pitch: target: *MIDDLE_PITCH_NORMAL - delay: 0.3 + delay: 0.6 - step: "lift arm" arm: joints: [ *JOINT1_READY_GET_STORED_POS, -1.2, -2.063, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ] @@ -1899,7 +2023,7 @@ steps_list: - step: "middle pitch up" middle_pitch: target: *MIDDLE_PITCH_NORMAL - delay: 0.3 + delay: 0.6 - step: "ready to get stored gold" arm: joints: [ *JOINT1_GET_STORED_GOLD_POS, *JOINT2_GET_STORED_GOLD_POS, *JOINT3_READY_GET_STORED_GOLD_POS, *JOINT4_R90_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ] @@ -1951,7 +2075,7 @@ steps_list: - step: "middle pitch up" middle_pitch: target: *MIDDLE_PITCH_NORMAL - delay: 0.3 + delay: 0.6 - step: "ready to get silver from gripper1" arm: joints: [ *JOINT1_READY_GET_STORED_POS, *JOINT2_GET_STORED_S1_POS, *JOINT3_GET_STORED_SILVER1_POS, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_GET_S1 ] @@ -1996,7 +2120,7 @@ steps_list: - step: "middle pitch up" middle_pitch: target: *MIDDLE_PITCH_NORMAL - delay: 0.3 + delay: 0.6 - step: "ready to get silver from gripper2" arm: joints: [ *JOINT1_READY_GET_STORED_POS, *JOINT2_GET_STORED_S2_POS, *JOINT3_GET_STORED_SILVER2_POS, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_GET_S2 ] @@ -2037,7 +2161,7 @@ steps_list: - step: "middle pitch up" middle_pitch: target: *MIDDLE_PITCH_NORMAL - delay: 0.3 + delay: 0.6 - step: "lift arm" arm: joints: [ *JOINT1_READY_GET_STORED_POS, 0.113, -2.063, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ] @@ -2085,7 +2209,7 @@ steps_list: - step: "middle pitch up" middle_pitch: target: *MIDDLE_PITCH_NORMAL - delay: 0.3 + delay: 0.6 - step: "ready to get stored gold" arm: joints: [ *JOINT1_GET_STORED_GOLD_POS, *JOINT2_GET_STORED_GOLD_POS, *JOINT3_READY_GET_STORED_GOLD_POS, *JOINT4_R90_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ] From cee21d47b030da345b8f8d8e371a4badd02dfa79 Mon Sep 17 00:00:00 2001 From: cc0h <7921166012@qq.com> Date: Fri, 9 Aug 2024 10:24:44 +0800 Subject: [PATCH 79/80] Disable chassis interface temporarily to avoid crazy chassis motion. --- engineer2_arm_config/launch/moveit.rviz | 84 +++++++++---------- .../include/engineer_middleware/middleware.h | 5 +- 2 files changed, 45 insertions(+), 44 deletions(-) diff --git a/engineer2_arm_config/launch/moveit.rviz b/engineer2_arm_config/launch/moveit.rviz index e70752e..2d517dd 100644 --- a/engineer2_arm_config/launch/moveit.rviz +++ b/engineer2_arm_config/launch/moveit.rviz @@ -4,6 +4,7 @@ Panels: Name: Displays Property Tree Widget: Expanded: + - /Global Options1 - /MotionPlanning1 - /TF1 - /TF1/Frames1 @@ -131,7 +132,7 @@ Visualization Manager: Frames: All Enabled: false base_link: - Value: false + Value: true fake_link4: Value: false gold_lifter_link: @@ -151,15 +152,15 @@ Visualization Manager: link4: Value: false link5: - Value: false + Value: true link6: Value: true map: - Value: false + Value: true odom: - Value: false + Value: true pitch: - Value: false + Value: true right_back_wheel: Value: false right_front_wheel: @@ -181,38 +182,37 @@ Visualization Manager: Show Axes: true Show Names: true Tree: - base_link: - gold_pusher_link: - gold_lifter_link: - {} - left_back_wheel: - {} - left_front_wheel: - {} - link1: - link2: - link3: - fake_link4: - {} - link4: - link5: - link6: - tools_link: - {} - yaw: - pitch: - {} - right_back_wheel: - {} - right_front_wheel: - {} - silver_lifter_link: - silver_pusher_link: - silver_gripper_link: - {} map: odom: - {} + base_link: + gold_pusher_link: + gold_lifter_link: + {} + left_back_wheel: + {} + left_front_wheel: + {} + link1: + link2: + link3: + fake_link4: + {} + link4: + link5: + link6: + tools_link: + {} + yaw: + pitch: + {} + right_back_wheel: + {} + right_front_wheel: + {} + silver_lifter_link: + silver_pusher_link: + silver_gripper_link: + {} Update Interval: 0 Value: true - Alpha: 1 @@ -334,7 +334,7 @@ Visualization Manager: Global Options: Background Color: 48; 48; 48 Default Light: true - Fixed Frame: base_link + Fixed Frame: odom Frame Rate: 30 Name: root Tools: @@ -346,7 +346,7 @@ Visualization Manager: Views: Current: Class: rviz/Orbit - Distance: 1.3629440069198608 + Distance: 1.3873012065887451 Enable Stereo Rendering: Stereo Eye Separation: 0.05999999865889549 Stereo Focal Distance: 1 @@ -354,17 +354,17 @@ Visualization Manager: Value: false Field of View: 0.75 Focal Point: - X: -0.3085601031780243 - Y: 0.24779054522514343 - Z: 0.4662521481513977 + X: 0.22694315016269684 + Y: -0.3219256103038788 + Z: 0.15961049497127533 Focal Shape Fixed Size: true Focal Shape Size: 0.05000000074505806 Invert Z Axis: false Name: Current View Near Clip Distance: 0.009999999776482582 - Pitch: 1.5697963237762451 + Pitch: 1.3847957849502563 Target Frame: base_link - Yaw: 3.154945135116577 + Yaw: 4.408130168914795 Saved: ~ Window Geometry: Displays: diff --git a/engineer_middleware/include/engineer_middleware/middleware.h b/engineer_middleware/include/engineer_middleware/middleware.h index 4d89de4..199935f 100644 --- a/engineer_middleware/include/engineer_middleware/middleware.h +++ b/engineer_middleware/include/engineer_middleware/middleware.h @@ -72,8 +72,9 @@ class Middleware } void run(ros::Duration period) { - if (is_middleware_control_) - chassis_interface_.run(period); + // TODO chassis run crazily in motion + // if (is_middleware_control_) + // chassis_interface_.run(period); } private: From c8a14d62707d80239f30680ececceda24a878fff Mon Sep 17 00:00:00 2001 From: cc0h <7921166012@qq.com> Date: Tue, 20 Aug 2024 21:30:14 +0800 Subject: [PATCH 80/80] Record. --- engineer_middleware/config/engineer2_steps_list.yaml | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/engineer_middleware/config/engineer2_steps_list.yaml b/engineer_middleware/config/engineer2_steps_list.yaml index 6521b06..ce4df3c 100644 --- a/engineer_middleware/config/engineer2_steps_list.yaml +++ b/engineer_middleware/config/engineer2_steps_list.yaml @@ -1217,11 +1217,11 @@ steps_list: MID_BIG_ISLAND0: - step: "get gold" gold_pusher: - target: -0.44 + target: -0.46 MID_BIG_ISLAND00: - step: "pull gold back" gold_pusher: - target: -0.42 + target: -0.43 - step: "lift gold" gold_lifter: target: 0.058 @@ -1391,7 +1391,7 @@ steps_list: BOTH_BIG_ISLAND0: - step: "get gold" gold_pusher: - target: -0.44 + target: -0.46 - step: "arm get gold" arm: joints: [ 0.040,0.147,-1.429,*JOINT4_R90_POS, 0.08, *JOINT6_HOME_POS ] @@ -1405,7 +1405,7 @@ steps_list: <<: *RIGHT_POS - step: "pull gold" gold_pusher: - target: -0.42 + target: -0.43 - step: "lift gold" gold_lifter: target: 0.058