Grow the graph for the tweak up/down buttons
We now have a ton of points between major nodes spaced such that we can
tweak up and down to adjust for the scale.
Change-Id: Id7db02b7be440bbb429995751a808c923f52ca98
diff --git a/y2018/control_loops/python/graph_generate.py b/y2018/control_loops/python/graph_generate.py
index 3b21e5d..b6fd3bd 100644
--- a/y2018/control_loops/python/graph_generate.py
+++ b/y2018/control_loops/python/graph_generate.py
@@ -386,7 +386,7 @@
for alpha in subdivide_spline(start, control1, control2, end)
])
- cr.move_to(self.start[0] + xy_end_circle_size, start[1])
+ cr.move_to(start[0] + xy_end_circle_size, start[1])
cr.arc(start[0], start[1], xy_end_circle_size, 0, 2.0 * numpy.pi)
cr.move_to(end[0] + xy_end_circle_size, end[1])
cr.arc(end[0], end[1], xy_end_circle_size, 0, 2.0 * numpy.pi)
@@ -557,7 +557,7 @@
front_switch_c2 = numpy.array([1.903841, -0.622351])
-front_points = [
+sparse_front_points = [
(front_high_box, "FrontHighBox"),
(front_middle2_box, "FrontMiddle2Box"),
(front_middle3_box, "FrontMiddle3Box"),
@@ -566,13 +566,58 @@
(front_switch, "FrontSwitch"),
] # yapf: disable
-back_points = [
+sparse_back_points = [
(back_high_box, "BackHighBox"),
(back_middle2_box, "BackMiddle2Box"),
(back_middle1_box, "BackMiddle1Box"),
(back_low_box, "BackLowBox"),
] # yapf: disable
+def expand_points(points, max_distance):
+ """Expands a list of points to be at most max_distance apart
+
+ Generates the paths to connect the new points to the closest input points,
+ and the paths connecting the points.
+
+ Args:
+ points, list of tuple of point, name, The points to start with and fill
+ in.
+ max_distance, float, The max distance between two points when expanding
+ the graph.
+
+ Return:
+ points, edges
+ """
+ result_points = [points[0]]
+ result_paths = []
+ for point, name in points[1:]:
+ previous_point = result_points[-1][0]
+ previous_point_xy = get_xy(previous_point)
+ circular_index = get_circular_index(previous_point)
+
+ point_xy = get_xy(point)
+ norm = numpy.linalg.norm(point_xy - previous_point_xy)
+ num_points = int(numpy.ceil(norm / max_distance))
+ last_iteration_point = previous_point
+ for subindex in range(1, num_points):
+ subpoint = to_theta(
+ alpha_blend(previous_point_xy, point_xy,
+ float(subindex) / num_points),
+ circular_index=circular_index)
+ result_points.append((subpoint, '%s%dof%d' % (name, subindex,
+ num_points)))
+ result_paths.append(XYSegment(previous_point, subpoint))
+ if (last_iteration_point != previous_point).any():
+ result_paths.append(XYSegment(last_iteration_point, subpoint))
+ result_paths.append(XYSegment(subpoint, point))
+ last_iteration_point = subpoint
+ result_points.append((point, name))
+
+ return result_points, result_paths
+
+front_points, front_paths = expand_points(sparse_front_points, 0.05)
+back_points, back_paths = expand_points(sparse_back_points, 0.05)
+
points = [(ready_above_box, "ReadyAboveBox"),
(tall_box_grab, "TallBoxGrab"),
(short_box_grab, "ShortBoxGrab"),
@@ -674,6 +719,6 @@
AngleSegment(up, below_hang),
AngleSegment(up, self_hang),
AngleSegment(up, partner_hang),
-]
+] + front_paths + back_paths
segments = named_segments + unnamed_segments