Gitiles
Code Review Sign In
realtimeroboticsgroup.org / RealtimeRoboticsGroup / test / f5123fee9ebe9b8af3764448528b1af132e36926 / . / y2023 / control_loops / python / graph_paths.py
blob: 26488bd0dd5a279bd6a787ab3ab0eacbb1bd6132 [file] [log] [blame]
Maxwell Hendersonf5123fe2023-02-04 13:44:41 -0800[diff] [blame^]1import numpy
2
3from graph_tools import *
4
5tall_box_x = 0.411
6tall_box_y = 0.125
7
8short_box_x = 0.431
9short_box_y = 0.082
10
11ready_above_box = to_theta_with_circular_index(tall_box_x,
12 tall_box_y + 0.08,
13 circular_index=-1)
14tall_box_grab = to_theta_with_circular_index(tall_box_x,
15 tall_box_y,
16 circular_index=-1)
17short_box_grab = to_theta_with_circular_index(short_box_x,
18 short_box_y,
19 circular_index=-1)
20
21# TODO(austin): Drive the front/back off the same numbers a bit better.
22front_high_box = to_theta_with_circular_index(0.378, 2.46, circular_index=-1)
23front_middle3_box = to_theta_with_circular_index(0.700,
24 2.125,
25 circular_index=-1.000000)
26front_middle2_box = to_theta_with_circular_index(0.700,
27 2.268,
28 circular_index=-1)
29front_middle1_box = to_theta_with_circular_index(0.800,
30 1.915,
31 circular_index=-1)
32front_low_box = to_theta_with_circular_index(0.87, 1.572, circular_index=-1)
33back_high_box = to_theta_with_circular_index(-0.75, 2.48, circular_index=0)
34back_middle2_box = to_theta_with_circular_index(-0.700, 2.27, circular_index=0)
35back_middle1_box = to_theta_with_circular_index(-0.800, 1.93, circular_index=0)
36back_low_box = to_theta_with_circular_index(-0.87, 1.64, circular_index=0)
37
38back_extra_low_box = to_theta_with_circular_index(-0.87,
39 1.52,
40 circular_index=0)
41
42front_switch = to_theta_with_circular_index(0.88, 0.967, circular_index=-1)
43back_switch = to_theta_with_circular_index(-0.88, 0.967, circular_index=-2)
44
45neutral = to_theta_with_circular_index(0.0, 0.33, circular_index=-1)
46
47up = to_theta_with_circular_index(0.0, 2.547, circular_index=-1)
48
49front_switch_auto = to_theta_with_circular_index(0.750,
50 2.20,
51 circular_index=-1.000000)
52
53duck = numpy.array([numpy.pi / 2.0 - 0.92, numpy.pi / 2.0 - 4.26])
54
55starting = numpy.array([numpy.pi / 2.0 - 0.593329, numpy.pi / 2.0 - 3.749631])
56vertical_starting = numpy.array([numpy.pi / 2.0, -numpy.pi / 2.0])
57
58self_hang = numpy.array([numpy.pi / 2.0 - 0.191611, numpy.pi / 2.0])
59partner_hang = numpy.array([numpy.pi / 2.0 - (-0.30), numpy.pi / 2.0])
60
61above_hang = numpy.array([numpy.pi / 2.0 - 0.14, numpy.pi / 2.0 - (-0.165)])
62below_hang = numpy.array([numpy.pi / 2.0 - 0.39, numpy.pi / 2.0 - (-0.517)])
63
64up_c1 = to_theta((0.63, 1.17), circular_index=-1)
65up_c2 = to_theta((0.65, 1.62), circular_index=-1)
66
67front_high_box_c1 = to_theta((0.63, 1.04), circular_index=-1)
68front_high_box_c2 = to_theta((0.50, 1.60), circular_index=-1)
69
70front_middle2_box_c1 = to_theta((0.41, 0.83), circular_index=-1)
71front_middle2_box_c2 = to_theta((0.52, 1.30), circular_index=-1)
72
73front_middle1_box_c1 = to_theta((0.34, 0.82), circular_index=-1)
74front_middle1_box_c2 = to_theta((0.48, 1.15), circular_index=-1)
75
76#c1: (1.421433, -1.070254)
77#c2: (1.434384, -1.057803
78ready_above_box_c1 = numpy.array([1.480802, -1.081218])
79ready_above_box_c2 = numpy.array([1.391449, -1.060331])
80
81front_switch_c1 = numpy.array([1.903841, -0.622351])
82front_switch_c2 = numpy.array([1.903841, -0.622351])
83
84sparse_front_points = [
85 (front_high_box, "FrontHighBox"),
86 (front_middle2_box, "FrontMiddle2Box"),
87 (front_middle3_box, "FrontMiddle3Box"),
88 (front_middle1_box, "FrontMiddle1Box"),
89 (front_low_box, "FrontLowBox"),
90 (front_switch, "FrontSwitch"),
91] # yapf: disable
92
93sparse_back_points = [
94 (back_high_box, "BackHighBox"),
95 (back_middle2_box, "BackMiddle2Box"),
96 (back_middle1_box, "BackMiddle1Box"),
97 (back_low_box, "BackLowBox"),
98 (back_extra_low_box, "BackExtraLowBox"),
99] # yapf: disable
100
101front_points, front_paths = expand_points(sparse_front_points, 0.06)
102back_points, back_paths = expand_points(sparse_back_points, 0.06)
103
104points = [(ready_above_box, "ReadyAboveBox"),
105 (tall_box_grab, "TallBoxGrab"),
106 (short_box_grab, "ShortBoxGrab"),
107 (back_switch, "BackSwitch"),
108 (neutral, "Neutral"),
109 (up, "Up"),
110 (above_hang, "AboveHang"),
111 (below_hang, "BelowHang"),
112 (self_hang, "SelfHang"),
113 (partner_hang, "PartnerHang"),
114 (front_switch_auto, "FrontSwitchAuto"),
115 (starting, "Starting"),
116 (duck, "Duck"),
117 (vertical_starting, "VerticalStarting"),
118] + front_points + back_points # yapf: disable
119
120duck_c1 = numpy.array([1.337111, -1.721008])
121duck_c2 = numpy.array([1.283701, -1.795519])
122
123ready_to_up_c1 = numpy.array([1.792962, 0.198329])
124ready_to_up_c2 = numpy.array([1.792962, 0.198329])
125
126front_switch_auto_c1 = numpy.array([1.792857, -0.372768])
127front_switch_auto_c2 = numpy.array([1.861885, -0.273664])
128
129# We need to define critical points so we can create paths connecting them.
130# TODO(austin): Attach velocities to the slow ones.
131ready_to_back_low_c1 = numpy.array([2.524325, 0.046417])
132
133neutral_to_back_low_c1 = numpy.array([2.381942, -0.070220])
134
135tall_to_back_low_c1 = numpy.array([2.603918, 0.088298])
136tall_to_back_low_c2 = numpy.array([1.605624, 1.003434])
137
138tall_to_back_high_c2 = numpy.array([1.508610, 0.946147])
139
140# If true, only plot the first named segment
141isolate = False
142
143long_alpha_unitizer = numpy.matrix([[1.0 / 17.0, 0.0], [0.0, 1.0 / 17.0]])
144
145neutral_to_back_c1 = numpy.array([0.702527, -2.618276])
146neutral_to_back_c2 = numpy.array([0.526914, -3.109691])
147
148named_segments = [
149 ThetaSplineSegment(neutral, neutral_to_back_c1, neutral_to_back_c2,
150 back_switch, "BackSwitch"),
151 ThetaSplineSegment(neutral,
152 neutral_to_back_low_c1,
153 tall_to_back_high_c2,
154 back_high_box,
155 "NeutralBoxToHigh",
156 alpha_unitizer=long_alpha_unitizer),
157 ThetaSplineSegment(neutral, neutral_to_back_low_c1, tall_to_back_high_c2,
158 back_middle2_box, "NeutralBoxToMiddle2",
159 long_alpha_unitizer),
160 ThetaSplineSegment(neutral, neutral_to_back_low_c1, tall_to_back_low_c2,
161 back_middle1_box, "NeutralBoxToMiddle1",
162 long_alpha_unitizer),
163 ThetaSplineSegment(neutral, neutral_to_back_low_c1, tall_to_back_low_c2,
164 back_low_box, "NeutralBoxToLow", long_alpha_unitizer),
165 ThetaSplineSegment(ready_above_box, ready_to_back_low_c1,
166 tall_to_back_high_c2, back_high_box, "ReadyBoxToHigh",
167 long_alpha_unitizer),
168 ThetaSplineSegment(ready_above_box, ready_to_back_low_c1,
169 tall_to_back_high_c2, back_middle2_box,
170 "ReadyBoxToMiddle2", long_alpha_unitizer),
171 ThetaSplineSegment(ready_above_box, ready_to_back_low_c1,
172 tall_to_back_low_c2, back_middle1_box,
173 "ReadyBoxToMiddle1", long_alpha_unitizer),
174 ThetaSplineSegment(ready_above_box, ready_to_back_low_c1,
175 tall_to_back_low_c2, back_low_box, "ReadyBoxToLow",
176 long_alpha_unitizer),
177 ThetaSplineSegment(short_box_grab, tall_to_back_low_c1,
178 tall_to_back_high_c2, back_high_box, "ShortBoxToHigh",
179 long_alpha_unitizer),
180 ThetaSplineSegment(short_box_grab, tall_to_back_low_c1,
181 tall_to_back_high_c2, back_middle2_box,
182 "ShortBoxToMiddle2", long_alpha_unitizer),
183 ThetaSplineSegment(short_box_grab, tall_to_back_low_c1,
184 tall_to_back_low_c2, back_middle1_box,
185 "ShortBoxToMiddle1", long_alpha_unitizer),
186 ThetaSplineSegment(short_box_grab, tall_to_back_low_c1,
187 tall_to_back_low_c2, back_low_box, "ShortBoxToLow",
188 long_alpha_unitizer),
189 ThetaSplineSegment(tall_box_grab, tall_to_back_low_c1,
190 tall_to_back_high_c2, back_high_box, "TallBoxToHigh",
191 long_alpha_unitizer),
192 ThetaSplineSegment(tall_box_grab, tall_to_back_low_c1,
193 tall_to_back_high_c2, back_middle2_box,
194 "TallBoxToMiddle2", long_alpha_unitizer),
195 ThetaSplineSegment(tall_box_grab, tall_to_back_low_c1, tall_to_back_low_c2,
196 back_middle1_box, "TallBoxToMiddle1",
197 long_alpha_unitizer),
198 ThetaSplineSegment(tall_box_grab, tall_to_back_low_c1, tall_to_back_low_c2,
199 back_low_box, "TallBoxToLow", long_alpha_unitizer),
200 SplineSegment(neutral, ready_above_box_c1, ready_above_box_c2,
201 ready_above_box, "ReadyToNeutral"),
202 XYSegment(ready_above_box, tall_box_grab, "ReadyToTallBox", vmax=6.0),
203 XYSegment(ready_above_box, short_box_grab, "ReadyToShortBox", vmax=6.0),
204 XYSegment(tall_box_grab, short_box_grab, "TallToShortBox", vmax=6.0),
205 SplineSegment(neutral, ready_above_box_c1, ready_above_box_c2,
206 tall_box_grab, "TallToNeutral"),
207 SplineSegment(neutral, ready_above_box_c1, ready_above_box_c2,
208 short_box_grab, "ShortToNeutral"),
209 SplineSegment(neutral, up_c1, up_c2, up, "NeutralToUp"),
210 SplineSegment(neutral, front_high_box_c1, front_high_box_c2,
211 front_high_box, "NeutralToFrontHigh"),
212 SplineSegment(neutral, front_middle2_box_c1, front_middle2_box_c2,
213 front_middle2_box, "NeutralToFrontMiddle2"),
214 SplineSegment(neutral, front_middle1_box_c1, front_middle1_box_c2,
215 front_middle1_box, "NeutralToFrontMiddle1"),
216]
217
218unnamed_segments = [
219 SplineSegment(neutral, front_switch_auto_c1, front_switch_auto_c2,
220 front_switch_auto),
221 SplineSegment(tall_box_grab, ready_to_up_c1, ready_to_up_c2, up),
222 SplineSegment(short_box_grab, ready_to_up_c1, ready_to_up_c2, up),
223 SplineSegment(ready_above_box, ready_to_up_c1, ready_to_up_c2, up),
224 ThetaSplineSegment(duck, duck_c1, duck_c2, neutral),
225 SplineSegment(neutral, front_switch_c1, front_switch_c2, front_switch),
226 XYSegment(ready_above_box, front_low_box),
227 XYSegment(ready_above_box, front_switch),
228 XYSegment(ready_above_box, front_middle1_box),
229 XYSegment(ready_above_box, front_middle2_box),
230 XYSegment(ready_above_box, front_middle3_box),
231 SplineSegment(ready_above_box, ready_to_up_c1, ready_to_up_c2,
232 front_high_box),
233 AngleSegment(starting, vertical_starting),
234 AngleSegment(vertical_starting, neutral),
235 XYSegment(neutral, front_low_box),
236 XYSegment(up, front_high_box),
237 XYSegment(up, front_middle2_box),
238 XYSegment(front_middle3_box, up),
239 XYSegment(front_middle3_box, front_high_box),
240 XYSegment(front_middle3_box, front_middle2_box),
241 XYSegment(front_middle3_box, front_middle1_box),
242 XYSegment(up, front_middle1_box),
243 XYSegment(up, front_low_box),
244 XYSegment(front_high_box, front_middle2_box),
245 XYSegment(front_high_box, front_middle1_box),
246 XYSegment(front_high_box, front_low_box),
247 XYSegment(front_middle2_box, front_middle1_box),
248 XYSegment(front_middle2_box, front_low_box),
249 XYSegment(front_middle1_box, front_low_box),
250 XYSegment(front_switch, front_low_box),
251 XYSegment(front_switch, up),
252 XYSegment(front_switch, front_high_box),
253 AngleSegment(up, back_high_box),
254 AngleSegment(up, back_middle2_box),
255 AngleSegment(up, back_middle1_box),
256 AngleSegment(up, back_low_box),
257 XYSegment(back_high_box, back_middle2_box),
258 XYSegment(back_high_box, back_middle1_box),
259 XYSegment(back_high_box, back_low_box),
260 XYSegment(back_middle2_box, back_middle1_box),
261 XYSegment(back_middle2_box, back_low_box),
262 XYSegment(back_middle1_box, back_low_box),
263 AngleSegment(up, above_hang),
264 AngleSegment(above_hang, below_hang),
265 AngleSegment(up, below_hang),
266 AngleSegment(up, self_hang),
267 AngleSegment(up, partner_hang),
268] + front_paths + back_paths
269
270segments = []
271if isolate:
272 segments += named_segments[:isolate]
273else:
274 segments += named_segments + unnamed_segments