Add hanger code.
This adds the goal nodes for the hanger, and the ability to hang.
Change-Id: Ib33c19298cd08e98abf27d9789982d78f8df191e
diff --git a/y2018/control_loops/python/graph_edit.py b/y2018/control_loops/python/graph_edit.py
index 183f7bf..fbf5f8b 100644
--- a/y2018/control_loops/python/graph_edit.py
+++ b/y2018/control_loops/python/graph_edit.py
@@ -93,7 +93,6 @@
pt = points[pt_i]
delta = last_pt[1] - pt[1]
if abs(delta) > numpy.pi:
- print(delta)
return points[pt_i:] + points[:pt_i]
last_pt = pt
return points
@@ -123,9 +122,6 @@
# Fully computed theta constrints.
lines_theta = list(p1.intersection(p2).exterior.coords)
-print("Theta constraint.")
-print(", ".join("{%s, %s}" % (a, b) for a, b in lines_theta))
-
lines1_theta_back = back_to_xy_loop(lines1_theta)
lines2_theta_back = back_to_xy_loop(lines2_theta)
@@ -419,8 +415,7 @@
def do_button_press(self, event):
self.last_pos = (event.x, event.y)
self.now_segment_pt = self.cur_pt_in_theta()
- print('Clicked at theta: (%f, %f)' % (self.now_segment_pt[0],
- self.now_segment_pt[1]))
+ print('Clicked at theta: %s' % (repr(self.now_segment_pt,)))
if not self.theta_version:
print('Clicked at xy, circular index: (%f, %f, %f)' %
(self.last_pos[0], self.last_pos[1],
diff --git a/y2018/control_loops/python/graph_generate.py b/y2018/control_loops/python/graph_generate.py
index a976145..6d7b628 100644
--- a/y2018/control_loops/python/graph_generate.py
+++ b/y2018/control_loops/python/graph_generate.py
@@ -420,6 +420,16 @@
up = to_theta_with_circular_index(0.0, 2.547, circular_index=-1)
+self_hang = numpy.array(
+ [numpy.pi / 2.0 - 0.191611, numpy.pi / 2.0])
+partner_hang = numpy.array(
+ [numpy.pi / 2.0 - (-0.25), numpy.pi / 2.0])
+
+above_hang = numpy.array(
+ [numpy.pi / 2.0 - 0.008739, numpy.pi / 2.0 - (-0.101927)])
+below_hang = numpy.array(
+ [numpy.pi / 2.0 - 0.329954, numpy.pi / 2.0 - (-0.534816)])
+
up_c1 = to_theta((0.63, 1.17), circular_index=-1)
up_c2 = to_theta((0.65, 1.62), circular_index=-1)
@@ -452,7 +462,12 @@
(front_switch, "FrontSwitch"),
(back_switch, "BackSwitch"),
(neutral, "Neutral"),
- (up, "Up")] # yapf: disable
+ (up, "Up"),
+ (above_hang, "AboveHang"),
+ (below_hang, "BelowHang"),
+ (self_hang, "SelfHang"),
+ (partner_hang, "PartnerHang"),
+] # yapf: disable
# We need to define critical points so we can create paths connecting them.
# TODO(austin): Attach velocities to the slow ones.
@@ -477,7 +492,6 @@
unnamed_segments = [
AngleSegment(neutral, back_switch),
- #XYSegment(neutral, front_switch),
SplineSegment(neutral, front_switch_c1, front_switch_c2, front_switch),
XYSegment(neutral, front_low_box),
@@ -503,6 +517,12 @@
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),
]
segments = named_segments + unnamed_segments