Remove y2018 paths and add placeholder paths
Signed-off-by: Maxwell Henderson <mxwhenderson@gmail.com>
Change-Id: I877ba5b83fa425a005016d0df571537eb07fb085
diff --git a/y2023/control_loops/python/graph_paths.py b/y2023/control_loops/python/graph_paths.py
index 26488bd..da0ad4f 100644
--- a/y2023/control_loops/python/graph_paths.py
+++ b/y2023/control_loops/python/graph_paths.py
@@ -2,273 +2,25 @@
from graph_tools import *
-tall_box_x = 0.411
-tall_box_y = 0.125
+neutral = to_theta_with_circular_index(-0.2, 0.33, circular_index=-1)
+zero = to_theta_with_circular_index(0.0, 0.0, circular_index=-1)
-short_box_x = 0.431
-short_box_y = 0.082
+neutral_to_cone_1 = to_theta_with_circular_index(0.0, 0.7, circular_index=-1)
+neutral_to_cone_2 = to_theta_with_circular_index(0.2, 0.5, circular_index=-1)
+cone_pos = to_theta_with_circular_index(1.0, 0.4, circular_index=-1)
-ready_above_box = to_theta_with_circular_index(tall_box_x,
- tall_box_y + 0.08,
- circular_index=-1)
-tall_box_grab = to_theta_with_circular_index(tall_box_x,
- tall_box_y,
- circular_index=-1)
-short_box_grab = to_theta_with_circular_index(short_box_x,
- short_box_y,
- circular_index=-1)
+neutral_to_cone_perch_pos_1 = to_theta_with_circular_index(0.4,
+ 1.0,
+ circular_index=-1)
+neutral_to_cone_perch_pos_2 = to_theta_with_circular_index(0.7,
+ 1.5,
+ circular_index=-1)
+cone_perch_pos = to_theta_with_circular_index(1.0, 2.0, circular_index=-1)
-# TODO(austin): Drive the front/back off the same numbers a bit better.
-front_high_box = to_theta_with_circular_index(0.378, 2.46, circular_index=-1)
-front_middle3_box = to_theta_with_circular_index(0.700,
- 2.125,
- circular_index=-1.000000)
-front_middle2_box = to_theta_with_circular_index(0.700,
- 2.268,
- circular_index=-1)
-front_middle1_box = to_theta_with_circular_index(0.800,
- 1.915,
- circular_index=-1)
-front_low_box = to_theta_with_circular_index(0.87, 1.572, circular_index=-1)
-back_high_box = to_theta_with_circular_index(-0.75, 2.48, circular_index=0)
-back_middle2_box = to_theta_with_circular_index(-0.700, 2.27, circular_index=0)
-back_middle1_box = to_theta_with_circular_index(-0.800, 1.93, circular_index=0)
-back_low_box = to_theta_with_circular_index(-0.87, 1.64, circular_index=0)
-
-back_extra_low_box = to_theta_with_circular_index(-0.87,
- 1.52,
- circular_index=0)
-
-front_switch = to_theta_with_circular_index(0.88, 0.967, circular_index=-1)
-back_switch = to_theta_with_circular_index(-0.88, 0.967, circular_index=-2)
-
-neutral = to_theta_with_circular_index(0.0, 0.33, circular_index=-1)
-
-up = to_theta_with_circular_index(0.0, 2.547, circular_index=-1)
-
-front_switch_auto = to_theta_with_circular_index(0.750,
- 2.20,
- circular_index=-1.000000)
-
-duck = numpy.array([numpy.pi / 2.0 - 0.92, numpy.pi / 2.0 - 4.26])
-
-starting = numpy.array([numpy.pi / 2.0 - 0.593329, numpy.pi / 2.0 - 3.749631])
-vertical_starting = numpy.array([numpy.pi / 2.0, -numpy.pi / 2.0])
-
-self_hang = numpy.array([numpy.pi / 2.0 - 0.191611, numpy.pi / 2.0])
-partner_hang = numpy.array([numpy.pi / 2.0 - (-0.30), numpy.pi / 2.0])
-
-above_hang = numpy.array([numpy.pi / 2.0 - 0.14, numpy.pi / 2.0 - (-0.165)])
-below_hang = numpy.array([numpy.pi / 2.0 - 0.39, numpy.pi / 2.0 - (-0.517)])
-
-up_c1 = to_theta((0.63, 1.17), circular_index=-1)
-up_c2 = to_theta((0.65, 1.62), circular_index=-1)
-
-front_high_box_c1 = to_theta((0.63, 1.04), circular_index=-1)
-front_high_box_c2 = to_theta((0.50, 1.60), circular_index=-1)
-
-front_middle2_box_c1 = to_theta((0.41, 0.83), circular_index=-1)
-front_middle2_box_c2 = to_theta((0.52, 1.30), circular_index=-1)
-
-front_middle1_box_c1 = to_theta((0.34, 0.82), circular_index=-1)
-front_middle1_box_c2 = to_theta((0.48, 1.15), circular_index=-1)
-
-#c1: (1.421433, -1.070254)
-#c2: (1.434384, -1.057803
-ready_above_box_c1 = numpy.array([1.480802, -1.081218])
-ready_above_box_c2 = numpy.array([1.391449, -1.060331])
-
-front_switch_c1 = numpy.array([1.903841, -0.622351])
-front_switch_c2 = numpy.array([1.903841, -0.622351])
-
-sparse_front_points = [
- (front_high_box, "FrontHighBox"),
- (front_middle2_box, "FrontMiddle2Box"),
- (front_middle3_box, "FrontMiddle3Box"),
- (front_middle1_box, "FrontMiddle1Box"),
- (front_low_box, "FrontLowBox"),
- (front_switch, "FrontSwitch"),
-] # yapf: disable
-
-sparse_back_points = [
- (back_high_box, "BackHighBox"),
- (back_middle2_box, "BackMiddle2Box"),
- (back_middle1_box, "BackMiddle1Box"),
- (back_low_box, "BackLowBox"),
- (back_extra_low_box, "BackExtraLowBox"),
-] # yapf: disable
-
-front_points, front_paths = expand_points(sparse_front_points, 0.06)
-back_points, back_paths = expand_points(sparse_back_points, 0.06)
-
-points = [(ready_above_box, "ReadyAboveBox"),
- (tall_box_grab, "TallBoxGrab"),
- (short_box_grab, "ShortBoxGrab"),
- (back_switch, "BackSwitch"),
- (neutral, "Neutral"),
- (up, "Up"),
- (above_hang, "AboveHang"),
- (below_hang, "BelowHang"),
- (self_hang, "SelfHang"),
- (partner_hang, "PartnerHang"),
- (front_switch_auto, "FrontSwitchAuto"),
- (starting, "Starting"),
- (duck, "Duck"),
- (vertical_starting, "VerticalStarting"),
-] + front_points + back_points # yapf: disable
-
-duck_c1 = numpy.array([1.337111, -1.721008])
-duck_c2 = numpy.array([1.283701, -1.795519])
-
-ready_to_up_c1 = numpy.array([1.792962, 0.198329])
-ready_to_up_c2 = numpy.array([1.792962, 0.198329])
-
-front_switch_auto_c1 = numpy.array([1.792857, -0.372768])
-front_switch_auto_c2 = numpy.array([1.861885, -0.273664])
-
-# We need to define critical points so we can create paths connecting them.
-# TODO(austin): Attach velocities to the slow ones.
-ready_to_back_low_c1 = numpy.array([2.524325, 0.046417])
-
-neutral_to_back_low_c1 = numpy.array([2.381942, -0.070220])
-
-tall_to_back_low_c1 = numpy.array([2.603918, 0.088298])
-tall_to_back_low_c2 = numpy.array([1.605624, 1.003434])
-
-tall_to_back_high_c2 = numpy.array([1.508610, 0.946147])
-
-# If true, only plot the first named segment
-isolate = False
-
-long_alpha_unitizer = numpy.matrix([[1.0 / 17.0, 0.0], [0.0, 1.0 / 17.0]])
-
-neutral_to_back_c1 = numpy.array([0.702527, -2.618276])
-neutral_to_back_c2 = numpy.array([0.526914, -3.109691])
-
-named_segments = [
- ThetaSplineSegment(neutral, neutral_to_back_c1, neutral_to_back_c2,
- back_switch, "BackSwitch"),
- ThetaSplineSegment(neutral,
- neutral_to_back_low_c1,
- tall_to_back_high_c2,
- back_high_box,
- "NeutralBoxToHigh",
- alpha_unitizer=long_alpha_unitizer),
- ThetaSplineSegment(neutral, neutral_to_back_low_c1, tall_to_back_high_c2,
- back_middle2_box, "NeutralBoxToMiddle2",
- long_alpha_unitizer),
- ThetaSplineSegment(neutral, neutral_to_back_low_c1, tall_to_back_low_c2,
- back_middle1_box, "NeutralBoxToMiddle1",
- long_alpha_unitizer),
- ThetaSplineSegment(neutral, neutral_to_back_low_c1, tall_to_back_low_c2,
- back_low_box, "NeutralBoxToLow", long_alpha_unitizer),
- ThetaSplineSegment(ready_above_box, ready_to_back_low_c1,
- tall_to_back_high_c2, back_high_box, "ReadyBoxToHigh",
- long_alpha_unitizer),
- ThetaSplineSegment(ready_above_box, ready_to_back_low_c1,
- tall_to_back_high_c2, back_middle2_box,
- "ReadyBoxToMiddle2", long_alpha_unitizer),
- ThetaSplineSegment(ready_above_box, ready_to_back_low_c1,
- tall_to_back_low_c2, back_middle1_box,
- "ReadyBoxToMiddle1", long_alpha_unitizer),
- ThetaSplineSegment(ready_above_box, ready_to_back_low_c1,
- tall_to_back_low_c2, back_low_box, "ReadyBoxToLow",
- long_alpha_unitizer),
- ThetaSplineSegment(short_box_grab, tall_to_back_low_c1,
- tall_to_back_high_c2, back_high_box, "ShortBoxToHigh",
- long_alpha_unitizer),
- ThetaSplineSegment(short_box_grab, tall_to_back_low_c1,
- tall_to_back_high_c2, back_middle2_box,
- "ShortBoxToMiddle2", long_alpha_unitizer),
- ThetaSplineSegment(short_box_grab, tall_to_back_low_c1,
- tall_to_back_low_c2, back_middle1_box,
- "ShortBoxToMiddle1", long_alpha_unitizer),
- ThetaSplineSegment(short_box_grab, tall_to_back_low_c1,
- tall_to_back_low_c2, back_low_box, "ShortBoxToLow",
- long_alpha_unitizer),
- ThetaSplineSegment(tall_box_grab, tall_to_back_low_c1,
- tall_to_back_high_c2, back_high_box, "TallBoxToHigh",
- long_alpha_unitizer),
- ThetaSplineSegment(tall_box_grab, tall_to_back_low_c1,
- tall_to_back_high_c2, back_middle2_box,
- "TallBoxToMiddle2", long_alpha_unitizer),
- ThetaSplineSegment(tall_box_grab, tall_to_back_low_c1, tall_to_back_low_c2,
- back_middle1_box, "TallBoxToMiddle1",
- long_alpha_unitizer),
- ThetaSplineSegment(tall_box_grab, tall_to_back_low_c1, tall_to_back_low_c2,
- back_low_box, "TallBoxToLow", long_alpha_unitizer),
- SplineSegment(neutral, ready_above_box_c1, ready_above_box_c2,
- ready_above_box, "ReadyToNeutral"),
- XYSegment(ready_above_box, tall_box_grab, "ReadyToTallBox", vmax=6.0),
- XYSegment(ready_above_box, short_box_grab, "ReadyToShortBox", vmax=6.0),
- XYSegment(tall_box_grab, short_box_grab, "TallToShortBox", vmax=6.0),
- SplineSegment(neutral, ready_above_box_c1, ready_above_box_c2,
- tall_box_grab, "TallToNeutral"),
- SplineSegment(neutral, ready_above_box_c1, ready_above_box_c2,
- short_box_grab, "ShortToNeutral"),
- SplineSegment(neutral, up_c1, up_c2, up, "NeutralToUp"),
- SplineSegment(neutral, front_high_box_c1, front_high_box_c2,
- front_high_box, "NeutralToFrontHigh"),
- SplineSegment(neutral, front_middle2_box_c1, front_middle2_box_c2,
- front_middle2_box, "NeutralToFrontMiddle2"),
- SplineSegment(neutral, front_middle1_box_c1, front_middle1_box_c2,
- front_middle1_box, "NeutralToFrontMiddle1"),
+segments = [
+ ThetaSplineSegment(neutral, neutral_to_cone_1, neutral_to_cone_2, cone_pos,
+ "NeutralToCone"),
+ ThetaSplineSegment(neutral, neutral_to_cone_perch_pos_1,
+ neutral_to_cone_perch_pos_2, cone_perch_pos,
+ "NeutralToPerchedCone"),
]
-
-unnamed_segments = [
- SplineSegment(neutral, front_switch_auto_c1, front_switch_auto_c2,
- front_switch_auto),
- SplineSegment(tall_box_grab, ready_to_up_c1, ready_to_up_c2, up),
- SplineSegment(short_box_grab, ready_to_up_c1, ready_to_up_c2, up),
- SplineSegment(ready_above_box, ready_to_up_c1, ready_to_up_c2, up),
- ThetaSplineSegment(duck, duck_c1, duck_c2, neutral),
- SplineSegment(neutral, front_switch_c1, front_switch_c2, front_switch),
- XYSegment(ready_above_box, front_low_box),
- XYSegment(ready_above_box, front_switch),
- XYSegment(ready_above_box, front_middle1_box),
- XYSegment(ready_above_box, front_middle2_box),
- XYSegment(ready_above_box, front_middle3_box),
- SplineSegment(ready_above_box, ready_to_up_c1, ready_to_up_c2,
- front_high_box),
- AngleSegment(starting, vertical_starting),
- AngleSegment(vertical_starting, neutral),
- XYSegment(neutral, front_low_box),
- XYSegment(up, front_high_box),
- XYSegment(up, front_middle2_box),
- XYSegment(front_middle3_box, up),
- XYSegment(front_middle3_box, front_high_box),
- XYSegment(front_middle3_box, front_middle2_box),
- XYSegment(front_middle3_box, front_middle1_box),
- XYSegment(up, front_middle1_box),
- XYSegment(up, front_low_box),
- XYSegment(front_high_box, front_middle2_box),
- XYSegment(front_high_box, front_middle1_box),
- XYSegment(front_high_box, front_low_box),
- XYSegment(front_middle2_box, front_middle1_box),
- XYSegment(front_middle2_box, front_low_box),
- XYSegment(front_middle1_box, front_low_box),
- XYSegment(front_switch, front_low_box),
- XYSegment(front_switch, up),
- XYSegment(front_switch, front_high_box),
- AngleSegment(up, back_high_box),
- AngleSegment(up, back_middle2_box),
- AngleSegment(up, back_middle1_box),
- AngleSegment(up, back_low_box),
- XYSegment(back_high_box, back_middle2_box),
- XYSegment(back_high_box, back_middle1_box),
- XYSegment(back_high_box, back_low_box),
- XYSegment(back_middle2_box, back_middle1_box),
- XYSegment(back_middle2_box, back_low_box),
- XYSegment(back_middle1_box, back_low_box),
- AngleSegment(up, above_hang),
- AngleSegment(above_hang, below_hang),
- AngleSegment(up, below_hang),
- AngleSegment(up, self_hang),
- AngleSegment(up, partner_hang),
-] + front_paths + back_paths
-
-segments = []
-if isolate:
- segments += named_segments[:isolate]
-else:
- segments += named_segments + unnamed_segments
\ No newline at end of file