| milind-u | 18a901d | 2023-02-17 21:51:55 -0800 | [diff] [blame] | 1 | import numpy as np |
| Maxwell Henderson | f5123fe | 2023-02-04 13:44:41 -0800 | [diff] [blame] | 2 | |
| milind-u | 18a901d | 2023-02-17 21:51:55 -0800 | [diff] [blame] | 3 | from y2023.control_loops.python.graph_tools import * |
| Maxwell Henderson | f5123fe | 2023-02-04 13:44:41 -0800 | [diff] [blame] | 4 | |
| milind-u | 18a901d | 2023-02-17 21:51:55 -0800 | [diff] [blame] | 5 | neutral = to_theta_with_circular_index_and_roll(joint_center[0], |
| 6 | joint_center[1] + l2 - l1, |
| milind-u | 060e4cf | 2023-02-22 00:08:52 -0800 | [diff] [blame] | 7 | 0.0, |
| milind-u | eeb08c5 | 2023-02-21 22:30:16 -0800 | [diff] [blame] | 8 | circular_index=1) |
| Maxwell Henderson | f5123fe | 2023-02-04 13:44:41 -0800 | [diff] [blame] | 9 | |
| Maxwell Henderson | 1ac7aac | 2023-02-23 17:35:32 -0800 | [diff] [blame] | 10 | # NeutralToGroundPickupBackConeUp |
| 11 | neutral_to_cone_up_1 = np.array([3.170156, -0.561227]) |
| 12 | neutral_to_cone_up_2 = np.array([2.972776, -1.026820]) |
| 13 | ground_pickup_back_cone_up = to_theta_with_circular_index_and_roll( |
| 14 | -0.913162844198605, 0.35, np.pi / 2.0, circular_index=1) |
| 15 | |
| 16 | # NeutralToGroundPickupBackConeDown |
| 17 | neutral_to_ground_pickup_back_cone_down_1 = np.array([3.170156, -0.561227]) |
| 18 | neutral_to_ground_pickup_back_cone_down_2 = np.array([2.972776, -1.026820]) |
| 19 | ground_pickup_back_cone_down = to_theta_with_circular_index_and_roll( |
| 20 | -0.95, 0.24, np.pi / 2.0, circular_index=1) |
| 21 | |
| 22 | # NeutralToBackMidConeUpScore |
| 23 | neutral_to_score_back_mid_cone_up_1 = np.array([0.994244, -1.417442]) |
| 24 | neutral_to_score_back_mid_cone_up_2 = np.array([1.711325, -0.679748]) |
| 25 | score_back_mid_cone_up_pos = to_theta_with_circular_index_and_roll( |
| 26 | -1.255555, 1.10, np.pi / 2.0, circular_index=0) |
| 27 | |
| 28 | # NeutralToMidConeDownScore |
| 29 | neutral_to_score_mid_cone_down_1 = np.array([3.394572, -0.239378]) |
| 30 | neutral_to_score_mid_cone_down_2 = np.array([3.654854, -0.626835]) |
| 31 | score_mid_cone_down_pos = to_theta_with_circular_index_and_roll( |
| 32 | -1.23, 0.74, np.pi / 2.0, circular_index=1) |
| 33 | |
| 34 | # NeutralToMidConeDownScore |
| 35 | neutral_to_hp_pickup_back_cone_up_1 = np.array([2.0, -0.239378]) |
| 36 | neutral_to_hp_pickup_back_cone_up_2 = np.array([1.6, -0.626835]) |
| 37 | neutral_to_hp_pickup_back_cone_up_alpha_rolls = [ |
| 38 | (0.7, 0.0), |
| 39 | (.9, np.pi / 2.0), |
| 40 | ] |
| 41 | hp_pickup_back_cone_up = to_theta_with_circular_index_and_roll( |
| 42 | -0.94, 1.31, np.pi / 2.0, circular_index=0) |
| 43 | |
| 44 | # NeutralToFrontHighConeUpScore |
| 45 | neutral_to_score_front_high_cone_up_1 = np.array([2.594244, 0.417442]) |
| 46 | neutral_to_score_front_high_cone_up_2 = np.array([1.51325, 0.679748]) |
| 47 | score_front_high_cone_up_pos = to_theta_with_circular_index_and_roll( |
| 48 | 0.87, 1.26, -np.pi / 2.0, circular_index=0) |
| 49 | |
| 50 | # NeutralToFrontMidConeUpScore |
| 51 | neutral_to_score_front_mid_cone_up_1 = np.array([3.0, 0.317442]) |
| 52 | neutral_to_score_front_mid_cone_up_2 = np.array([2.9, 0.479748]) |
| 53 | score_front_mid_cone_up_pos = to_theta_with_circular_index_and_roll( |
| 54 | 0.34, 0.93, -np.pi / 2.0, circular_index=0) |
| 55 | |
| 56 | neutral_to_cone_down_1 = np.array([2.396694, 0.508020]) |
| 57 | neutral_to_cone_down_2 = np.array([2.874513, 0.933160]) |
| 58 | cone_down_pos = to_theta_with_circular_index_and_roll(0.7, |
| 59 | 0.11, |
| 60 | np.pi / 2.0, |
| 61 | circular_index=0) |
| 62 | |
| 63 | neutral_to_cube_1 = np.array([2.396694, 0.508020]) |
| 64 | neutral_to_cube_2 = np.array([2.874513, 0.933160]) |
| 65 | |
| 66 | cube_pos = to_theta_with_circular_index_and_roll(0.7, |
| 67 | 0.24, |
| 68 | np.pi / 2.0, |
| 69 | circular_index=0) |
| 70 | |
| 71 | neutral_to_pickup_control_alpha_rolls = [ |
| 72 | (0.30, 0.0), |
| 73 | (.95, np.pi / 2.0), |
| 74 | ] |
| Austin Schuh | 76dd4e7 | 2023-02-22 21:23:57 -0800 | [diff] [blame] | 75 | |
| 76 | neutral_to_score_1 = np.array([0.994244, -1.417442]) |
| 77 | neutral_to_score_2 = np.array([1.711325, -0.679748]) |
| 78 | |
| Maxwell Henderson | 1ac7aac | 2023-02-23 17:35:32 -0800 | [diff] [blame] | 79 | score_low_pos = to_theta_with_circular_index_and_roll(-(0.41 / 2 + 0.49), |
| 80 | 0 + 0.05, |
| 81 | np.pi / 2.0, |
| 82 | circular_index=1) |
| Maxwell Henderson | f5123fe | 2023-02-04 13:44:41 -0800 | [diff] [blame] | 83 | |
| Maxwell Henderson | 1ac7aac | 2023-02-23 17:35:32 -0800 | [diff] [blame] | 84 | neutral_to_score_low_2 = np.array([3.37926599, -0.73664663]) |
| 85 | |
| 86 | score_mid_cube_pos = to_theta_with_circular_index_and_roll(-(0.58 + 0.49), |
| 87 | 0.6 + 0.05, |
| 88 | np.pi / 2.0, |
| 89 | circular_index=0) |
| 90 | |
| 91 | score_high_cone_pos = to_theta_with_circular_index_and_roll(-1.01, |
| 92 | 1.17 + 0.05, |
| 93 | np.pi / 2.0, |
| 94 | circular_index=0) |
| 95 | |
| 96 | score_high_cube_pos = to_theta_with_circular_index_and_roll(-1.01, |
| 97 | 0.90 + 0.05, |
| 98 | np.pi / 2.0, |
| 99 | circular_index=0) |
| 100 | |
| 101 | neutral_to_back_score_control_alpha_rolls = [(0.40, 0.0), (.95, np.pi / 2.0)] |
| 102 | neutral_to_front_score_control_alpha_rolls = [(0.40, 0.0), (.95, -np.pi / 2.0)] |
| 103 | |
| 104 | points = [(neutral, "NeutralPos"), |
| 105 | (ground_pickup_back_cone_up, "GroundPickupBackConeUp"), |
| 106 | (ground_pickup_back_cone_down, "GroundPickupBackConeDown"), |
| 107 | (hp_pickup_back_cone_up, "HPPickupBackConeUp"), |
| 108 | (cone_down_pos, "ConeDownPos"), (score_low_pos, "ScoreLowPos"), |
| 109 | (score_back_mid_cone_up_pos, "ScoreBackMidConeUpPos"), |
| 110 | (score_front_high_cone_up_pos, "ScoreFrontHighConeUpPos"), |
| 111 | (score_front_mid_cone_up_pos, "ScoreFrontMidConeUpPos"), |
| 112 | (score_mid_cone_down_pos, "ScoreBackMidConeDownPos"), |
| 113 | (score_mid_cube_pos, "ScoreMidCubePos"), |
| 114 | (score_high_cone_pos, "ScoreHighConePos"), |
| 115 | (score_high_cube_pos, "ScoreHighCubePos"), (cube_pos, "CubePos")] |
| Maxwell Henderson | 83cf6d6 | 2023-02-10 20:29:26 -0800 | [diff] [blame] | 116 | front_points = [] |
| 117 | back_points = [] |
| 118 | unnamed_segments = [] |
| milind-u | 3738518 | 2023-02-20 15:07:28 -0800 | [diff] [blame] | 119 | named_segments = [ |
| Maxwell Henderson | 1ac7aac | 2023-02-23 17:35:32 -0800 | [diff] [blame] | 120 | ThetaSplineSegment("NeutralToGroundPickupBackConeUp", neutral, |
| 121 | neutral_to_cone_up_1, neutral_to_cone_up_2, |
| 122 | ground_pickup_back_cone_up, |
| milind-u | 18a901d | 2023-02-17 21:51:55 -0800 | [diff] [blame] | 123 | neutral_to_pickup_control_alpha_rolls), |
| Maxwell Henderson | 1ac7aac | 2023-02-23 17:35:32 -0800 | [diff] [blame] | 124 | ThetaSplineSegment("NeutralToGroundPickupBackConeDown", neutral, |
| 125 | neutral_to_ground_pickup_back_cone_down_1, |
| 126 | neutral_to_ground_pickup_back_cone_down_2, |
| 127 | ground_pickup_back_cone_down, |
| 128 | neutral_to_pickup_control_alpha_rolls), |
| 129 | ThetaSplineSegment("NeutralToHPPickupBackConeUp", neutral, |
| 130 | neutral_to_hp_pickup_back_cone_up_1, |
| 131 | neutral_to_hp_pickup_back_cone_up_2, |
| 132 | hp_pickup_back_cone_up, |
| 133 | neutral_to_hp_pickup_back_cone_up_alpha_rolls), |
| 134 | ThetaSplineSegment("NeutralToFrontHighConeUpScore", neutral, |
| 135 | neutral_to_score_front_high_cone_up_1, |
| 136 | neutral_to_score_front_high_cone_up_2, |
| 137 | score_front_high_cone_up_pos, |
| 138 | neutral_to_front_score_control_alpha_rolls), |
| 139 | ThetaSplineSegment("NeutralToFrontMidConeUpScore", neutral, |
| 140 | neutral_to_score_front_mid_cone_up_1, |
| 141 | neutral_to_score_front_mid_cone_up_2, |
| 142 | score_front_mid_cone_up_pos, |
| 143 | neutral_to_front_score_control_alpha_rolls), |
| 144 | ThetaSplineSegment("NeutralToConeDown", neutral, neutral_to_cone_down_1, |
| 145 | neutral_to_cone_down_2, cone_down_pos, |
| 146 | neutral_to_pickup_control_alpha_rolls), |
| 147 | ThetaSplineSegment("NeutralToCube", neutral, neutral_to_cube_1, |
| 148 | neutral_to_cube_2, cube_pos, |
| 149 | neutral_to_pickup_control_alpha_rolls), |
| 150 | ThetaSplineSegment("NeutralToLowScore", neutral, neutral_to_score_1, |
| 151 | neutral_to_score_low_2, score_low_pos, |
| 152 | neutral_to_back_score_control_alpha_rolls), |
| 153 | ThetaSplineSegment("NeutralToBackMidConeUpScore", neutral, |
| 154 | neutral_to_score_back_mid_cone_up_1, |
| 155 | neutral_to_score_back_mid_cone_up_2, |
| 156 | score_back_mid_cone_up_pos, |
| 157 | neutral_to_back_score_control_alpha_rolls), |
| 158 | ThetaSplineSegment("NeutralToMidConeDownScore", neutral, |
| 159 | neutral_to_score_mid_cone_down_1, |
| 160 | neutral_to_score_mid_cone_down_2, |
| 161 | score_mid_cone_down_pos, |
| 162 | neutral_to_back_score_control_alpha_rolls), |
| 163 | ThetaSplineSegment("NeutralToMidCubeScore", neutral, neutral_to_score_1, |
| 164 | neutral_to_score_2, score_mid_cube_pos, |
| 165 | neutral_to_back_score_control_alpha_rolls), |
| 166 | ThetaSplineSegment("NeutralToHighConeScore", neutral, neutral_to_score_1, |
| 167 | neutral_to_score_2, score_high_cone_pos, |
| 168 | neutral_to_back_score_control_alpha_rolls), |
| 169 | ThetaSplineSegment("NeutralToHighCubeScore", neutral, neutral_to_score_1, |
| 170 | neutral_to_score_2, score_high_cube_pos, |
| 171 | neutral_to_back_score_control_alpha_rolls), |
| Maxwell Henderson | f5123fe | 2023-02-04 13:44:41 -0800 | [diff] [blame] | 172 | ] |
| milind-u | 3738518 | 2023-02-20 15:07:28 -0800 | [diff] [blame] | 173 | |
| milind-u | 18a901d | 2023-02-17 21:51:55 -0800 | [diff] [blame] | 174 | segments = named_segments + unnamed_segments |