Copy y2018 python arm visualization to y2023
Signed-off-by: Maxwell Henderson <mxwhenderson@gmail.com>
Change-Id: I0e4c2be36e46ab1d88ba04cfdb8e80f1e88ec5fc
diff --git a/y2023/control_loops/python/graph_generate.py b/y2023/control_loops/python/graph_generate.py
new file mode 100644
index 0000000..046b9dd
--- /dev/null
+++ b/y2023/control_loops/python/graph_generate.py
@@ -0,0 +1,798 @@
+import numpy
+
+# joint_center in x-y space.
+joint_center = (-0.299, 0.299)
+
+# Joint distances (l1 = "proximal", l2 = "distal")
+l1 = 46.25 * 0.0254
+l2 = 43.75 * 0.0254
+
+
+# Convert from x-y coordinates to theta coordinates.
+# orientation is a bool. This orientation is circular_index mod 2.
+# where circular_index is the circular index, or the position in the
+# "hyperextension" zones. "cross_point" allows shifting the place where
+# it rounds the result so that it draws nicer (no other functional differences).
+def to_theta(pt, circular_index, cross_point=-numpy.pi):
+ orient = (circular_index % 2) == 0
+ x = pt[0]
+ y = pt[1]
+ x -= joint_center[0]
+ y -= joint_center[1]
+ l3 = numpy.hypot(x, y)
+ t3 = numpy.arctan2(y, x)
+ theta1 = numpy.arccos((l1**2 + l3**2 - l2**2) / (2 * l1 * l3))
+
+ if orient:
+ theta1 = -theta1
+ theta1 += t3
+ theta1 = (theta1 - cross_point) % (2 * numpy.pi) + cross_point
+ theta2 = numpy.arctan2(y - l1 * numpy.sin(theta1),
+ x - l1 * numpy.cos(theta1))
+ return numpy.array((theta1, theta2))
+
+
+# Simple trig to go back from theta1, theta2 to x-y
+def to_xy(theta1, theta2):
+ x = numpy.cos(theta1) * l1 + numpy.cos(theta2) * l2 + joint_center[0]
+ y = numpy.sin(theta1) * l1 + numpy.sin(theta2) * l2 + joint_center[1]
+ orient = ((theta2 - theta1) % (2.0 * numpy.pi)) < numpy.pi
+ return (x, y, orient)
+
+
+def get_circular_index(theta):
+ return int(numpy.floor((theta[1] - theta[0]) / numpy.pi))
+
+
+def get_xy(theta):
+ theta1 = theta[0]
+ theta2 = theta[1]
+ x = numpy.cos(theta1) * l1 + numpy.cos(theta2) * l2 + joint_center[0]
+ y = numpy.sin(theta1) * l1 + numpy.sin(theta2) * l2 + joint_center[1]
+ return numpy.array((x, y))
+
+
+# Draw a list of lines to a cairo context.
+def draw_lines(cr, lines):
+ cr.move_to(lines[0][0], lines[0][1])
+ for pt in lines[1:]:
+ cr.line_to(pt[0], pt[1])
+
+
+max_dist = 0.01
+max_dist_theta = numpy.pi / 64
+xy_end_circle_size = 0.01
+theta_end_circle_size = 0.07
+
+
+# Subdivide in theta space.
+def subdivide_theta(lines):
+ out = []
+ last_pt = lines[0]
+ out.append(last_pt)
+ for n_pt in lines[1:]:
+ for pt in subdivide(last_pt, n_pt, max_dist_theta):
+ out.append(pt)
+ last_pt = n_pt
+
+ return out
+
+
+# subdivide in xy space.
+def subdivide_xy(lines, max_dist=max_dist):
+ out = []
+ last_pt = lines[0]
+ out.append(last_pt)
+ for n_pt in lines[1:]:
+ for pt in subdivide(last_pt, n_pt, max_dist):
+ out.append(pt)
+ last_pt = n_pt
+
+ return out
+
+
+def to_theta_with_ci(pt, circular_index):
+ return to_theta_with_circular_index(pt[0], pt[1], circular_index)
+
+
+# to_theta, but distinguishes between
+def to_theta_with_circular_index(x, y, circular_index):
+ theta1, theta2 = to_theta((x, y), circular_index)
+ n_circular_index = int(numpy.floor((theta2 - theta1) / numpy.pi))
+ theta2 = theta2 + ((circular_index - n_circular_index)) * numpy.pi
+ return numpy.array((theta1, theta2))
+
+
+# alpha is in [0, 1] and is the weight to merge a and b.
+def alpha_blend(a, b, alpha):
+ """Blends a and b.
+
+ Args:
+ alpha: double, Ratio. Needs to be in [0, 1] and is the weight to blend a
+ and b.
+ """
+ return b * alpha + (1.0 - alpha) * a
+
+
+def normalize(v):
+ """Normalize a vector while handling 0 length vectors."""
+ norm = numpy.linalg.norm(v)
+ if norm == 0:
+ return v
+ return v / norm
+
+
+# CI is circular index and allows selecting between all the stats that map
+# to the same x-y state (by giving them an integer index).
+# This will compute approximate first and second derivatives with respect
+# to path length.
+def to_theta_with_circular_index_and_derivs(x, y, dx, dy,
+ circular_index_select):
+ a = to_theta_with_circular_index(x, y, circular_index_select)
+ b = to_theta_with_circular_index(x + dx * 0.0001, y + dy * 0.0001,
+ circular_index_select)
+ c = to_theta_with_circular_index(x - dx * 0.0001, y - dy * 0.0001,
+ circular_index_select)
+ d1 = normalize(b - a)
+ d2 = normalize(c - a)
+ accel = (d1 + d2) / numpy.linalg.norm(a - b)
+ return (a[0], a[1], d1[0], d1[1], accel[0], accel[1])
+
+
+def to_theta_with_ci_and_derivs(p_prev, p, p_next, c_i_select):
+ a = to_theta(p, c_i_select)
+ b = to_theta(p_next, c_i_select)
+ c = to_theta(p_prev, c_i_select)
+ d1 = normalize(b - a)
+ d2 = normalize(c - a)
+ accel = (d1 + d2) / numpy.linalg.norm(a - b)
+ return (a[0], a[1], d1[0], d1[1], accel[0], accel[1])
+
+
+# Generic subdivision algorithm.
+def subdivide(p1, p2, max_dist):
+ dx = p2[0] - p1[0]
+ dy = p2[1] - p1[1]
+ dist = numpy.sqrt(dx**2 + dy**2)
+ n = int(numpy.ceil(dist / max_dist))
+ return [(alpha_blend(p1[0], p2[0],
+ float(i) / n), alpha_blend(p1[1], p2[1],
+ float(i) / n))
+ for i in range(1, n + 1)]
+
+
+# convert from an xy space loop into a theta loop.
+# All segements are expected go from one "hyper-extension" boundary
+# to another, thus we must go backwards over the "loop" to get a loop in
+# x-y space.
+def to_theta_loop(lines, cross_point=-numpy.pi):
+ out = []
+ last_pt = lines[0]
+ for n_pt in lines[1:]:
+ for pt in subdivide(last_pt, n_pt, max_dist):
+ out.append(to_theta(pt, 0, cross_point))
+ last_pt = n_pt
+ for n_pt in reversed(lines[:-1]):
+ for pt in subdivide(last_pt, n_pt, max_dist):
+ out.append(to_theta(pt, 1, cross_point))
+ last_pt = n_pt
+ return out
+
+
+# Convert a loop (list of line segments) into
+# The name incorrectly suggests that it is cyclic.
+def back_to_xy_loop(lines):
+ out = []
+ last_pt = lines[0]
+ out.append(to_xy(last_pt[0], last_pt[1]))
+ for n_pt in lines[1:]:
+ for pt in subdivide(last_pt, n_pt, max_dist_theta):
+ out.append(to_xy(pt[0], pt[1]))
+ last_pt = n_pt
+
+ return out
+
+
+# Segment in angle space.
+class AngleSegment:
+
+ def __init__(self, start, end, name=None, alpha_unitizer=None, vmax=None):
+ """Creates an angle segment.
+
+ Args:
+ start: (double, double), The start of the segment in theta1, theta2
+ coordinates in radians
+ end: (double, double), The end of the segment in theta1, theta2
+ coordinates in radians
+ """
+ self.start = start
+ self.end = end
+ self.name = name
+ self.alpha_unitizer = alpha_unitizer
+ self.vmax = vmax
+
+ def __repr__(self):
+ return "AngleSegment(%s, %s)" % (repr(self.start), repr(self.end))
+
+ def DrawTo(self, cr, theta_version):
+ if theta_version:
+ cr.move_to(self.start[0], self.start[1] + theta_end_circle_size)
+ cr.arc(self.start[0], self.start[1], theta_end_circle_size, 0,
+ 2.0 * numpy.pi)
+ cr.move_to(self.end[0], self.end[1] + theta_end_circle_size)
+ cr.arc(self.end[0], self.end[1], theta_end_circle_size, 0,
+ 2.0 * numpy.pi)
+ cr.move_to(self.start[0], self.start[1])
+ cr.line_to(self.end[0], self.end[1])
+ else:
+ start_xy = to_xy(self.start[0], self.start[1])
+ end_xy = to_xy(self.end[0], self.end[1])
+ draw_lines(cr, back_to_xy_loop([self.start, self.end]))
+ cr.move_to(start_xy[0] + xy_end_circle_size, start_xy[1])
+ cr.arc(start_xy[0], start_xy[1], xy_end_circle_size, 0,
+ 2.0 * numpy.pi)
+ cr.move_to(end_xy[0] + xy_end_circle_size, end_xy[1])
+ cr.arc(end_xy[0], end_xy[1], xy_end_circle_size, 0, 2.0 * numpy.pi)
+
+ def ToThetaPoints(self):
+ dx = self.end[0] - self.start[0]
+ dy = self.end[1] - self.start[1]
+ mag = numpy.hypot(dx, dy)
+ dx /= mag
+ dy /= mag
+
+ return [(self.start[0], self.start[1], dx, dy, 0.0, 0.0),
+ (self.end[0], self.end[1], dx, dy, 0.0, 0.0)]
+
+
+class XYSegment:
+ """Straight line in XY space."""
+
+ def __init__(self, start, end, name=None, alpha_unitizer=None, vmax=None):
+ """Creates an XY segment.
+
+ Args:
+ start: (double, double), The start of the segment in theta1, theta2
+ coordinates in radians
+ end: (double, double), The end of the segment in theta1, theta2
+ coordinates in radians
+ """
+ self.start = start
+ self.end = end
+ self.name = name
+ self.alpha_unitizer = alpha_unitizer
+ self.vmax = vmax
+
+ def __repr__(self):
+ return "XYSegment(%s, %s)" % (repr(self.start), repr(self.end))
+
+ def DrawTo(self, cr, theta_version):
+ if theta_version:
+ theta1, theta2 = self.start
+ circular_index_select = int(
+ numpy.floor((self.start[1] - self.start[0]) / numpy.pi))
+ start = get_xy(self.start)
+ end = get_xy(self.end)
+
+ ln = [(start[0], start[1]), (end[0], end[1])]
+ draw_lines(cr, [
+ to_theta_with_circular_index(x, y, circular_index_select)
+ for x, y in subdivide_xy(ln)
+ ])
+ cr.move_to(self.start[0] + theta_end_circle_size, self.start[1])
+ cr.arc(self.start[0], self.start[1], theta_end_circle_size, 0,
+ 2.0 * numpy.pi)
+ cr.move_to(self.end[0] + theta_end_circle_size, self.end[1])
+ cr.arc(self.end[0], self.end[1], theta_end_circle_size, 0,
+ 2.0 * numpy.pi)
+ else:
+ start = get_xy(self.start)
+ end = get_xy(self.end)
+ cr.move_to(start[0], start[1])
+ cr.line_to(end[0], end[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)
+
+ def ToThetaPoints(self):
+ """ Converts to points in theta space via to_theta_with_circular_index_and_derivs"""
+ theta1, theta2 = self.start
+ circular_index_select = int(
+ numpy.floor((self.start[1] - self.start[0]) / numpy.pi))
+ start = get_xy(self.start)
+ end = get_xy(self.end)
+
+ ln = [(start[0], start[1]), (end[0], end[1])]
+
+ dx = end[0] - start[0]
+ dy = end[1] - start[1]
+ mag = numpy.hypot(dx, dy)
+ dx /= mag
+ dy /= mag
+
+ return [
+ to_theta_with_circular_index_and_derivs(x, y, dx, dy,
+ circular_index_select)
+ for x, y in subdivide_xy(ln, 0.01)
+ ]
+
+
+def spline_eval(start, control1, control2, end, alpha):
+ a = alpha_blend(start, control1, alpha)
+ b = alpha_blend(control1, control2, alpha)
+ c = alpha_blend(control2, end, alpha)
+ return alpha_blend(alpha_blend(a, b, alpha), alpha_blend(b, c, alpha),
+ alpha)
+
+
+def subdivide_spline(start, control1, control2, end):
+ # TODO: pick N based on spline parameters? or otherwise change it to be more evenly spaced?
+ n = 100
+ for i in range(0, n + 1):
+ yield i / float(n)
+
+
+class SplineSegment:
+
+ def __init__(self,
+ start,
+ control1,
+ control2,
+ end,
+ name=None,
+ alpha_unitizer=None,
+ vmax=None):
+ self.start = start
+ self.control1 = control1
+ self.control2 = control2
+ self.end = end
+ self.name = name
+ self.alpha_unitizer = alpha_unitizer
+ self.vmax = vmax
+
+ def __repr__(self):
+ return "SplineSegment(%s, %s, %s, %s)" % (repr(
+ self.start), repr(self.control1), repr(
+ self.control2), repr(self.end))
+
+ def DrawTo(self, cr, theta_version):
+ if theta_version:
+ c_i_select = get_circular_index(self.start)
+ start = get_xy(self.start)
+ control1 = get_xy(self.control1)
+ control2 = get_xy(self.control2)
+ end = get_xy(self.end)
+
+ draw_lines(cr, [
+ to_theta(spline_eval(start, control1, control2, end, alpha),
+ c_i_select)
+ for alpha in subdivide_spline(start, control1, control2, end)
+ ])
+ cr.move_to(self.start[0] + theta_end_circle_size, self.start[1])
+ cr.arc(self.start[0], self.start[1], theta_end_circle_size, 0,
+ 2.0 * numpy.pi)
+ cr.move_to(self.end[0] + theta_end_circle_size, self.end[1])
+ cr.arc(self.end[0], self.end[1], theta_end_circle_size, 0,
+ 2.0 * numpy.pi)
+ else:
+ start = get_xy(self.start)
+ control1 = get_xy(self.control1)
+ control2 = get_xy(self.control2)
+ end = get_xy(self.end)
+
+ draw_lines(cr, [
+ spline_eval(start, control1, control2, end, alpha)
+ for alpha in subdivide_spline(start, control1, control2, end)
+ ])
+
+ 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)
+
+ def ToThetaPoints(self):
+ t1, t2 = self.start
+ c_i_select = get_circular_index(self.start)
+ start = get_xy(self.start)
+ control1 = get_xy(self.control1)
+ control2 = get_xy(self.control2)
+ end = get_xy(self.end)
+
+ return [
+ to_theta_with_ci_and_derivs(
+ spline_eval(start, control1, control2, end, alpha - 0.00001),
+ spline_eval(start, control1, control2, end, alpha),
+ spline_eval(start, control1, control2, end, alpha + 0.00001),
+ c_i_select)
+ for alpha in subdivide_spline(start, control1, control2, end)
+ ]
+
+
+def get_derivs(t_prev, t, t_next):
+ c, a, b = t_prev, t, t_next
+ d1 = normalize(b - a)
+ d2 = normalize(c - a)
+ accel = (d1 + d2) / numpy.linalg.norm(a - b)
+ return (a[0], a[1], d1[0], d1[1], accel[0], accel[1])
+
+
+class ThetaSplineSegment:
+
+ def __init__(self,
+ start,
+ control1,
+ control2,
+ end,
+ name=None,
+ alpha_unitizer=None,
+ vmax=None):
+ self.start = start
+ self.control1 = control1
+ self.control2 = control2
+ self.end = end
+ self.name = name
+ self.alpha_unitizer = alpha_unitizer
+ self.vmax = vmax
+
+ def __repr__(self):
+ return "ThetaSplineSegment(%s, %s, &s, %s)" % (repr(
+ self.start), repr(self.control1), repr(
+ self.control2), repr(self.end))
+
+ def DrawTo(self, cr, theta_version):
+ if (theta_version):
+ draw_lines(cr, [
+ spline_eval(self.start, self.control1, self.control2, self.end,
+ alpha)
+ for alpha in subdivide_spline(self.start, self.control1,
+ self.control2, self.end)
+ ])
+ else:
+ start = get_xy(self.start)
+ end = get_xy(self.end)
+
+ draw_lines(cr, [
+ get_xy(
+ spline_eval(self.start, self.control1, self.control2,
+ self.end, alpha))
+ for alpha in subdivide_spline(self.start, self.control1,
+ self.control2, self.end)
+ ])
+
+ 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)
+
+ def ToThetaPoints(self):
+ return [
+ get_derivs(
+ spline_eval(self.start, self.control1, self.control2, self.end,
+ alpha - 0.00001),
+ spline_eval(self.start, self.control1, self.control2, self.end,
+ alpha),
+ spline_eval(self.start, self.control1, self.control2, self.end,
+ alpha + 0.00001))
+ for alpha in subdivide_spline(self.start, self.control1,
+ self.control2, self.end)
+ ]
+
+
+tall_box_x = 0.411
+tall_box_y = 0.125
+
+short_box_x = 0.431
+short_box_y = 0.082
+
+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)
+
+# 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
+
+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(last_iteration_point, subpoint, vmax=6.0))
+ if (last_iteration_point != previous_point).any():
+ result_paths.append(XYSegment(previous_point, subpoint))
+ if subindex == num_points - 1:
+ result_paths.append(XYSegment(subpoint, point, vmax=6.0))
+ else:
+ 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.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"),
+]
+
+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