Added more paths to graph edit
Changed variable names to be more descriptive, added paths, converted to
metric.
Change-Id: Ic8474b567d1794082fe275a5e1a86e14688acb53
diff --git a/y2018/control_loops/python/BUILD b/y2018/control_loops/python/BUILD
index 5a44834..eea64ad 100644
--- a/y2018/control_loops/python/BUILD
+++ b/y2018/control_loops/python/BUILD
@@ -96,3 +96,15 @@
"//third_party/matplotlib-cpp",
],
)
+
+py_binary(
+ name = "graph_edit",
+ srcs = [
+ "basic_window.py",
+ "graph_edit.py",
+ "graph_generate.py",
+ ],
+ # Sigh, these aren't respected.
+ default_python_version = "PY3",
+ srcs_version = "PY3",
+)
diff --git a/y2018/control_loops/python/basic_window.py b/y2018/control_loops/python/basic_window.py
index 7d93fe1..7caf299 100644
--- a/y2018/control_loops/python/basic_window.py
+++ b/y2018/control_loops/python/basic_window.py
@@ -8,108 +8,115 @@
identity = cairo.Matrix()
+
# Override the matrix of a cairo context.
class OverrideMatrix(object):
- def __init__(self, cr, matrix):
- self.cr = cr
- self.matrix = matrix
- def __enter__(self):
- self.cr.save()
- self.cr.set_matrix(self.matrix)
+ def __init__(self, cr, matrix):
+ self.cr = cr
+ self.matrix = matrix
- def __exit__(self, type, value, traceback):
- self.cr.restore();
+ def __enter__(self):
+ self.cr.save()
+ self.cr.set_matrix(self.matrix)
+
+ def __exit__(self, type, value, traceback):
+ self.cr.restore()
+
mainloop = GLib.MainLoop()
+
def quit_main_loop(*args):
- mainloop.quit()
+ mainloop.quit()
+
def RunApp():
- try:
- mainloop.run();
- except KeyboardInterrupt:
- print('\nCtrl+C hit, quitting')
- mainloop.quit()
+ try:
+ mainloop.run()
+ except KeyboardInterrupt:
+ print('\nCtrl+C hit, quitting')
+ mainloop.quit()
+
# Create a GTK+ widget on which we will draw using Cairo
class BaseWindow(Gtk.DrawingArea):
- def method_connect(self, event, cb):
- def handler(obj, *args):
- cb(*args)
- self.window.connect(event, handler)
+ def method_connect(self, event, cb):
+ def handler(obj, *args):
+ cb(*args)
- # Draw in response to an expose-event
- def __init__(self):
- super().__init__()
- self.window = Gtk.Window()
- self.window.set_title("DrawingArea")
- self.window.connect("destroy", quit_main_loop)
- self.window.set_events(Gdk.EventMask.BUTTON_PRESS_MASK |
- Gdk.EventMask.BUTTON_RELEASE_MASK |
- Gdk.EventMask.POINTER_MOTION_MASK |
- Gdk.EventMask.SCROLL_MASK |
- Gdk.EventMask.KEY_PRESS_MASK
- )
- self.method_connect("key-press-event", self.do_key_press)
- self.method_connect("button-press-event", self._do_button_press_internal)
- self.method_connect("configure-event", self._do_configure)
+ self.window.connect(event, handler)
- self.set_size_request(640, 400) #640 * 2, 1229)
- self.window.add(self)
- self.window.show_all()
- self.center = (0, 0)
- self.shape = (640, 400)
- self.needs_redraw = False
+ # Draw in response to an expose-event
+ def __init__(self):
+ super(BaseWindow, self).__init__()
+ self.window = Gtk.Window()
+ self.window.set_title("DrawingArea")
+ self.window.connect("destroy", quit_main_loop)
+ self.window.set_events(Gdk.EventMask.BUTTON_PRESS_MASK
+ | Gdk.EventMask.BUTTON_RELEASE_MASK
+ | Gdk.EventMask.POINTER_MOTION_MASK
+ | Gdk.EventMask.SCROLL_MASK
+ | Gdk.EventMask.KEY_PRESS_MASK)
+ self.method_connect("key-press-event", self.do_key_press)
+ self.method_connect("button-press-event",
+ self._do_button_press_internal)
+ self.method_connect("configure-event", self._do_configure)
- def get_current_scale(self):
- w_w, w_h = self.window_shape
- w, h = self.shape
- return min((w_w / w), (w_h / h))
+ self.set_size_request(640, 400)
+ self.window.add(self)
+ self.window.show_all()
+ self.center = (0, 0)
+ self.shape = (640, 400)
+ self.needs_redraw = False
- def init_extents(self, center, shape):
- self.center = center
- self.shape = shape
+ def get_current_scale(self):
+ w_w, w_h = self.window_shape
+ w, h = self.shape
+ return min((w_w / w), (w_h / h))
- # The gtk system creates cr which is a cairo_context_t (in the c docs), and then it
- # passes it as a function argument to the "draw" event. do_draw is the default name.
- def do_draw(self, cr):
- cr.save()
- cr.set_font_size(20)
- cr.translate(self.window_shape[0] / 2, self.window_shape[1] / 2)
- scale = self.get_current_scale()
- cr.scale(scale, -scale)
- cr.translate(-self.center[0], -self.center[1])
- self.needs_redraw = False
- self.handle_draw(cr)
- cr.restore()
+ def init_extents(self, center, shape):
+ self.center = center
+ self.shape = shape
- # Handle the expose-event by drawing
- def handle_draw(self, cr):
- pass
+ # The gtk system creates cr which is a cairo_context_t (in the c docs), and then it
+ # passes it as a function argument to the "draw" event. do_draw is the default name.
+ def do_draw(self, cr):
+ cr.save()
+ cr.set_font_size(20)
+ cr.translate(self.window_shape[0] / 2, self.window_shape[1] / 2)
+ scale = self.get_current_scale()
+ cr.scale(scale, -scale)
+ cr.translate(-self.center[0], -self.center[1])
+ self.needs_redraw = False
+ self.handle_draw(cr)
+ cr.restore()
- def do_key_press(self, event):
- pass
+ # Handle the expose-event by drawing
+ def handle_draw(self, cr):
+ pass
- def _do_button_press_internal(self, event):
- o_x = event.x
- o_y = event.y
- x = event.x - self.window_shape[0] / 2
- y = self.window_shape[1] / 2 - event.y
- scale = self.get_current_scale()
- event.x = x / scale + self.center[0]
- event.y = y / scale + self.center[1]
- self.do_button_press(event)
- event.x = o_x
- event.y = o_y
+ def do_key_press(self, event):
+ pass
- def do_button_press(self, event):
- pass
+ def _do_button_press_internal(self, event):
+ o_x = event.x
+ o_y = event.y
+ x = event.x - self.window_shape[0] / 2
+ y = self.window_shape[1] / 2 - event.y
+ scale = self.get_current_scale()
+ event.x = x / scale + self.center[0]
+ event.y = y / scale + self.center[1]
+ self.do_button_press(event)
+ event.x = o_x
+ event.y = o_y
- def _do_configure(self, event):
- self.window_shape = (event.width, event.height)
+ def do_button_press(self, event):
+ pass
- def redraw(self):
- if not self.needs_redraw:
- self.needs_redraw = True
- self.window.queue_draw()
+ def _do_configure(self, event):
+ self.window_shape = (event.width, event.height)
+
+ def redraw(self):
+ if not self.needs_redraw:
+ self.needs_redraw = True
+ self.window.queue_draw()
diff --git a/y2018/control_loops/python/graph_edit.py b/y2018/control_loops/python/graph_edit.py
index 3386579..183f7bf 100644
--- a/y2018/control_loops/python/graph_edit.py
+++ b/y2018/control_loops/python/graph_edit.py
@@ -1,5 +1,7 @@
+from __future__ import print_function
import os
import basic_window
+import random
import gi
import numpy
gi.require_version('Gtk', '3.0')
@@ -15,83 +17,92 @@
import shapely
from shapely.geometry import Polygon
+
def px(cr):
- return OverrideMatrix(cr, identity)
+ return OverrideMatrix(cr, identity)
-# Draws a cross with fixed dimensions in pixel space.
+
def draw_px_cross(cr, length_px):
- with px(cr):
- x,y = cr.get_current_point()
- cr.move_to(x, y - length_px)
- cr.line_to(x, y + length_px)
- cr.stroke()
+ """Draws a cross with fixed dimensions in pixel space."""
+ with px(cr):
+ x, y = cr.get_current_point()
+ cr.move_to(x, y - length_px)
+ cr.line_to(x, y + length_px)
+ cr.stroke()
- cr.move_to(x - length_px, y)
- cr.line_to(x + length_px, y)
- cr.stroke()
+ cr.move_to(x - length_px, y)
+ cr.line_to(x + length_px, y)
+ cr.stroke()
-# Distance between two points in angle space.
+
def angle_dist_sqr(a1, a2):
- return (a1[0] - a2[0]) ** 2 + (a1[1] - a2[1]) ** 2
+ """Distance between two points in angle space."""
+ return (a1[0] - a2[0])**2 + (a1[1] - a2[1])**2
+
# Find the highest y position that intersects the vertical line defined by x.
def inter_y(x):
- return numpy.sqrt((l2 + l1) ** 2 - (x - joint_center[0]) ** 2) + joint_center[1]
+ return numpy.sqrt((l2 + l1)**2 -
+ (x - joint_center[0])**2) + joint_center[1]
+
# This is the x position where the inner (hyperextension) circle intersects the horizontal line
-derr = numpy.sqrt((l1 - l2) ** 2 - (joint_center[1] - 12.0) ** 2)
+derr = numpy.sqrt((l1 - l2)**2 - (joint_center[1] - 0.3048)**2)
+
# Define min and max l1 angles based on vertical constraints.
def get_angle(boundary):
- h = numpy.sqrt((l1) ** 2 - (boundary - joint_center[0]) ** 2) + joint_center[1]
- return numpy.arctan2(h, boundary - joint_center[0])
+ h = numpy.sqrt((l1)**2 - (boundary - joint_center[0])**2) + joint_center[1]
+ return numpy.arctan2(h, boundary - joint_center[0])
+
# left hand side lines
lines1 = [
- (-32.525, inter_y(-32.525)),
- (-32.525, 5.5),
- (-23.025, 5.5),
- (-23.025, 12.0),
- (joint_center[0] - derr, 12.0),
+ (-0.826135, inter_y(-0.826135)),
+ (-0.826135, 0.1397),
+ (-23.025 * 0.0254, 0.1397),
+ (-23.025 * 0.0254, 0.3048),
+ (joint_center[0] - derr, 0.3048),
]
# right hand side lines
-lines2 = [
- (joint_center[0] + derr, 12.0),
- (16.625, 12.0),
- (16.625, 5.5),
- (32.525, 5.5),
- (32.525, inter_y(32.525))
-]
+lines2 = [(joint_center[0] + derr, 0.3048), (0.422275, 0.3048),
+ (0.422275, 0.1397), (0.826135, 0.1397), (0.826135,
+ inter_y(0.826135))]
-t1_min = get_angle(32.525 - 4.0)
-t2_min = -7 / 4.0 * numpy.pi
+t1_min = get_angle((32.525 - 4.0) * 0.0254)
+t2_min = -7.0 / 4.0 * numpy.pi
-t1_max = get_angle(-32.525 + 4.0)
-t2_max = numpy.pi * 3 / 4.0
+t1_max = get_angle((-32.525 + 4.0) * 0.0254)
+t2_max = numpy.pi * 3.0 / 4.0
+
# Draw lines to cr + stroke.
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])
- with px(cr): cr.stroke()
+ cr.move_to(lines[0][0], lines[0][1])
+ for pt in lines[1:]:
+ cr.line_to(pt[0], pt[1])
+ with px(cr):
+ cr.stroke()
+
# Rotate a rasterized loop such that it aligns to when the parameters loop
def rotate_to_jump_point(points):
- last_pt = points[0]
- for pt_i in range(1, len(points)):
- 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
+ last_pt = points[0]
+ for pt_i in range(1, len(points)):
+ 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
+
# shift points vertically by dy.
def y_shift(points, dy):
- return [(x, y + dy) for x, y in points]
+ return [(x, y + dy) for x, y in points]
+
lines1_theta_part = rotate_to_jump_point(to_theta_loop(lines1, 0))
lines2_theta_part = rotate_to_jump_point(to_theta_loop(lines2))
@@ -106,297 +117,318 @@
p1 = Polygon(lines_theta)
-p2 = Polygon([(t1_min, t2_min), (t1_max, t2_min),
- (t1_max, t2_max), (t1_min, t2_max)])
+p2 = Polygon([(t1_min, t2_min), (t1_max, t2_min), (t1_max, t2_max), (t1_min,
+ t2_max)])
# Fully computed theta constrints.
lines_theta = list(p1.intersection(p2).exterior.coords)
-print(", ".join("{%s, %s}" % (a,b) for a, b in lines_theta))
+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)
lines_theta_back = back_to_xy_loop(lines_theta)
+
# Get the closest point to a line from a test pt.
def get_closest(prev, cur, pt):
- dx_ang = (cur[0] - prev[0])
- dy_ang = (cur[1] - prev[1])
+ dx_ang = (cur[0] - prev[0])
+ dy_ang = (cur[1] - prev[1])
- d = numpy.sqrt(dx_ang ** 2 + dy_ang ** 2)
- if (d < 0.000001):
- return prev, numpy.sqrt((prev[0] - pt[0]) ** 2 + (prev[1] - pt[1]) ** 2)
+ d = numpy.sqrt(dx_ang**2 + dy_ang**2)
+ if (d < 0.000001):
+ return prev, numpy.sqrt((prev[0] - pt[0])**2 + (prev[1] - pt[1])**2)
+
+ pdx = -dy_ang / d
+ pdy = dx_ang / d
+
+ dpx = pt[0] - prev[0]
+ dpy = pt[1] - prev[1]
+
+ alpha = (dx_ang * dpx + dy_ang * dpy) / d / d
+
+ if (alpha < 0):
+ return prev, numpy.sqrt((prev[0] - pt[0])**2 + (prev[1] - pt[1])**2)
+ elif (alpha > 1):
+ return cur, numpy.sqrt((cur[0] - pt[0])**2 + (cur[1] - pt[1])**2)
+ else:
+ return (alpha_blend(prev[0], cur[0], alpha), alpha_blend(prev[1], cur[1], alpha)), \
+ abs(dpx * pdx + dpy * pdy)
- pdx = -dy_ang / d
- pdy = dx_ang / d
-
- dpx = pt[0] - prev[0]
- dpy = pt[1] - prev[1]
-
- alpha = (dx_ang * dpx + dy_ang * dpy) / d / d
-
- if (alpha < 0):
- return prev, numpy.sqrt((prev[0] - pt[0]) ** 2 + (prev[1] - pt[1]) ** 2)
- elif (alpha > 1):
- return cur, numpy.sqrt((cur[0] - pt[0]) ** 2 + (cur[1] - pt[1]) ** 2)
- else:
- return (alpha_blend(prev[0], cur[0], alpha), alpha_blend(prev[1], cur[1], alpha)), \
- abs(dpx * pdx + dpy * pdy)
-
-#
+#
def closest_segment(lines, pt):
- c_pt, c_pt_dist = get_closest(lines[-1], lines[0], pt)
- for i in range(1, len(lines)):
- prev = lines[i - 1]
- cur = lines[i]
- c_pt_new, c_pt_new_dist = get_closest(prev, cur, pt)
- if c_pt_new_dist < c_pt_dist:
- c_pt = c_pt_new
- c_pt_dist = c_pt_new_dist
- return c_pt, c_pt_dist
+ c_pt, c_pt_dist = get_closest(lines[-1], lines[0], pt)
+ for i in range(1, len(lines)):
+ prev = lines[i - 1]
+ cur = lines[i]
+ c_pt_new, c_pt_new_dist = get_closest(prev, cur, pt)
+ if c_pt_new_dist < c_pt_dist:
+ c_pt = c_pt_new
+ c_pt_dist = c_pt_new_dist
+ return c_pt, c_pt_dist
+
# Create a GTK+ widget on which we will draw using Cairo
class Silly(basic_window.BaseWindow):
- def __init__(self):
- super().__init__()
+ def __init__(self):
+ super(Silly, self).__init__()
- self.theta_version = True
- self.reinit_extents()
+ self.theta_version = True
+ self.reinit_extents()
- self.last_pos = (20, 20)
- self.c_i_select = 0
- self.click_bool = False
+ self.last_pos = (numpy.pi / 2.0, 1.0)
+ self.circular_index_select = -1
+ # Extra stuff for drawing lines.
+ self.segments = []
+ self.prev_segment_pt = None
+ self.now_segment_pt = None
- # Extra stuff for drawing lines.
- self.segs = []
- self.prev_seg_pt = None
- self.now_seg_pt = None
-
- def reinit_extents(self):
- if self.theta_version:
- self.extents_x_min = -numpy.pi * 2
- self.extents_x_max = numpy.pi * 2
- self.extents_y_min = -numpy.pi * 2
- self.extents_y_max = numpy.pi * 2
- else:
- self.extents_x_min = -40.0
- self.extents_x_max = 40.0
- self.extents_y_min = -4.0
- self.extents_y_max = 110.0
-
- self.init_extents((0.5*(self.extents_x_min+self.extents_x_max), 0.5*(self.extents_y_max+self.extents_y_min)),
- (1.0*(self.extents_x_max-self.extents_x_min), 1.0*(self.extents_y_max-self.extents_y_min)))
-
- # Handle the expose-event by drawing
- def handle_draw(self, cr):
- # use "with px(cr): blah;" to transform to pixel coordinates.
-
- # Fill the background color of the window with grey
- cr.set_source_rgb(0.5, 0.5, 0.5)
- cr.paint()
-
- # Draw a extents rectangle
- cr.set_source_rgb(1.0, 1.0, 1.0)
- cr.rectangle(self.extents_x_min, self.extents_y_min,
- (self.extents_x_max-self.extents_x_min), self.extents_y_max-self.extents_y_min)
- cr.fill()
-
- if not self.theta_version:
-
- # Draw a filled white rectangle.
- cr.set_source_rgb(1.0, 1.0, 1.0)
- cr.rectangle(-2.0, -2.0, 4.0, 4.0)
- cr.fill()
-
- cr.set_source_rgb(0.0, 0.0, 1.0)
- cr.arc(joint_center[0], joint_center[1], l2 + l1, 0, 2 * numpy.pi)
- with px(cr): cr.stroke()
- cr.arc(joint_center[0], joint_center[1], l1 - l2, 0, 2 * numpy.pi)
- with px(cr): cr.stroke()
-
- else:
- # Draw a filled white rectangle.
- cr.set_source_rgb(1.0, 1.0, 1.0)
- cr.rectangle(-numpy.pi, -numpy.pi, numpy.pi * 2, numpy.pi * 2)
- cr.fill()
-
- if self.theta_version:
- cr.set_source_rgb(0.0, 0.0, 1.0)
- for i in range(-6, 6):
- cr.move_to(-40, -40 + i * numpy.pi)
- cr.line_to(40, 40 + i * numpy.pi)
- with px(cr): cr.stroke()
-
-
- if not self.theta_version:
- cr.set_source_rgb(0.2, 1.0, 0.2)
- draw_lines(cr, lines2)
-
- if self.theta_version:
- cr.set_source_rgb(0.5, 0.5, 1.0)
- draw_lines(cr, lines_theta)
-
- else:
- cr.set_source_rgb(0.5, 1.0, 1.0)
- draw_lines(cr, lines1)
- draw_lines(cr, lines2)
-
- def set_color(cr, c_i):
- if c_i == -2:
- cr.set_source_rgb(0.0, 0.25, 1.0)
- elif c_i == -1:
- cr.set_source_rgb(0.5, 0.0, 1.0)
- elif c_i == 0:
- cr.set_source_rgb(0.5, 1.0, 1.0)
- elif c_i == 1:
- cr.set_source_rgb(0.0, 0.5, 1.0)
- elif c_i == 2:
- cr.set_source_rgb(0.5, 1.0, 0.5)
+ def reinit_extents(self):
+ if self.theta_version:
+ self.extents_x_min = -numpy.pi * 2
+ self.extents_x_max = numpy.pi * 2
+ self.extents_y_min = -numpy.pi * 2
+ self.extents_y_max = numpy.pi * 2
else:
- cr.set_source_rgb(1.0, 0.0, 0.0)
+ self.extents_x_min = -40.0 * 0.0254
+ self.extents_x_max = 40.0 * 0.0254
+ self.extents_y_min = -4.0 * 0.0254
+ self.extents_y_max = 110.0 * 0.0254
- def get_ci(pt):
- t1, t2 = pt
- c_i = int(numpy.floor((t2 - t1) / numpy.pi))
- return c_i
+ self.init_extents(
+ (0.5 * (self.extents_x_min + self.extents_x_max), 0.5 *
+ (self.extents_y_max + self.extents_y_min)),
+ (1.0 * (self.extents_x_max - self.extents_x_min), 1.0 *
+ (self.extents_y_max - self.extents_y_min)))
- cr.set_source_rgb(0.0, 0.0, 1.0)
- lines = subdivide_theta(lines_theta)
- o_c_i = c_i = get_ci(lines[0])
- p_xy = to_xy(lines[0][0], lines[0][1])
- if c_i == self.c_i_select: cr.move_to(p_xy[0] + c_i * 0, p_xy[1])
- for pt in lines[1:]:
- p_xy = to_xy(pt[0], pt[1])
- c_i = get_ci(pt)
- if o_c_i == self.c_i_select: cr.line_to(p_xy[0] + o_c_i * 0, p_xy[1])
- if c_i != o_c_i:
- o_c_i = c_i
- with px(cr): cr.stroke()
- if c_i == self.c_i_select: cr.move_to(p_xy[0] + c_i * 0, p_xy[1])
+ # Handle the expose-event by drawing
+ def handle_draw(self, cr):
+ # use "with px(cr): blah;" to transform to pixel coordinates.
- with px(cr): cr.stroke()
+ # Fill the background color of the window with grey
+ cr.set_source_rgb(0.5, 0.5, 0.5)
+ cr.paint()
- if not self.theta_version:
- t1, t2 = to_theta(self.last_pos[0], self.last_pos[1], (self.c_i_select % 2) == 0)
- x, y = joint_center[0], joint_center[1]
- cr.move_to(x, y)
+ # Draw a extents rectangle
+ cr.set_source_rgb(1.0, 1.0, 1.0)
+ cr.rectangle(self.extents_x_min, self.extents_y_min,
+ (self.extents_x_max - self.extents_x_min),
+ self.extents_y_max - self.extents_y_min)
+ cr.fill()
- x += numpy.cos(t1) * l1
- y += numpy.sin(t1) * l1
- cr.line_to(x, y)
- x += numpy.cos(t2) * l2
- y += numpy.sin(t2) * l2
- cr.line_to(x, y)
- with px(cr): cr.stroke()
+ if not self.theta_version:
+ # Draw a filled white rectangle.
+ cr.set_source_rgb(1.0, 1.0, 1.0)
+ cr.rectangle(-2.0, -2.0, 4.0, 4.0)
+ cr.fill()
- cr.move_to(self.last_pos[0], self.last_pos[1])
- cr.set_source_rgb(0.0, 1.0, 0.2)
- draw_px_cross(cr, 20)
+ cr.set_source_rgb(0.0, 0.0, 1.0)
+ cr.arc(joint_center[0], joint_center[1], l2 + l1, 0,
+ 2.0 * numpy.pi)
+ with px(cr):
+ cr.stroke()
+ cr.arc(joint_center[0], joint_center[1], l1 - l2, 0,
+ 2.0 * numpy.pi)
+ with px(cr):
+ cr.stroke()
+ else:
+ # Draw a filled white rectangle.
+ cr.set_source_rgb(1.0, 1.0, 1.0)
+ cr.rectangle(-numpy.pi, -numpy.pi, numpy.pi * 2.0, numpy.pi * 2.0)
+ cr.fill()
- if self.theta_version:
- cr.set_source_rgb(0.0, 1.0, 0.2)
+ if self.theta_version:
+ cr.set_source_rgb(0.0, 0.0, 1.0)
+ for i in range(-6, 6):
+ cr.move_to(-40, -40 + i * numpy.pi)
+ cr.line_to(40, 40 + i * numpy.pi)
+ with px(cr):
+ cr.stroke()
- cr.set_source_rgb(0.0, 1.0, 0.2)
- cr.move_to(self.last_pos[0], self.last_pos[1])
- draw_px_cross(cr, 5)
+ if self.theta_version:
+ cr.set_source_rgb(0.5, 0.5, 1.0)
+ draw_lines(cr, lines_theta)
+ else:
+ cr.set_source_rgb(0.5, 1.0, 1.0)
+ draw_lines(cr, lines1)
+ draw_lines(cr, lines2)
- c_pt, dist = closest_segment(lines_theta, self.last_pos)
- print("dist:", dist, c_pt, self.last_pos)
- cr.set_source_rgb(0.0, 1.0, 1.0)
- cr.move_to(c_pt[0], c_pt[1])
- draw_px_cross(cr, 5)
+ def set_color(cr, circular_index):
+ if circular_index == -2:
+ cr.set_source_rgb(0.0, 0.25, 1.0)
+ elif circular_index == -1:
+ cr.set_source_rgb(0.5, 0.0, 1.0)
+ elif circular_index == 0:
+ cr.set_source_rgb(0.5, 1.0, 1.0)
+ elif circular_index == 1:
+ cr.set_source_rgb(0.0, 0.5, 1.0)
+ elif circular_index == 2:
+ cr.set_source_rgb(0.5, 1.0, 0.5)
+ else:
+ cr.set_source_rgb(1.0, 0.0, 0.0)
- cr.set_source_rgb(0.0, 0.5, 1.0)
- for seg in self.segs:
- seg.DrawTo(cr, self.theta_version)
- with px(cr): cr.stroke()
+ def get_circular_index(pt):
+ theta1, theta2 = pt
+ circular_index = int(numpy.floor((theta2 - theta1) / numpy.pi))
+ return circular_index
- cr.set_source_rgb(0.0, 1.0, 0.5)
- seg = self.current_seg()
- print(seg)
- if seg:
- seg.DrawTo(cr, self.theta_version)
- with px(cr): cr.stroke()
+ cr.set_source_rgb(0.0, 0.0, 1.0)
+ lines = subdivide_theta(lines_theta)
+ o_circular_index = circular_index = get_circular_index(lines[0])
+ p_xy = to_xy(lines[0][0], lines[0][1])
+ if circular_index == self.circular_index_select:
+ cr.move_to(p_xy[0] + circular_index * 0, p_xy[1])
+ for pt in lines[1:]:
+ p_xy = to_xy(pt[0], pt[1])
+ circular_index = get_circular_index(pt)
+ if o_circular_index == self.circular_index_select:
+ cr.line_to(p_xy[0] + o_circular_index * 0, p_xy[1])
+ if circular_index != o_circular_index:
+ o_circular_index = circular_index
+ with px(cr):
+ cr.stroke()
+ if circular_index == self.circular_index_select:
+ cr.move_to(p_xy[0] + circular_index * 0, p_xy[1])
- def cur_pt_in_theta(self):
- if self.theta_version: return self.last_pos
- t1, t2 = to_theta(self.last_pos[0], self.last_pos[1], (self.c_i_select % 2) == 0)
- n_ci = int(numpy.floor((t2 - t1) / numpy.pi))
- t2 = t2 + ((self.c_i_select - n_ci)) * numpy.pi
- return (t1, t2)
+ with px(cr):
+ cr.stroke()
- # Current seg based on which mode the drawing system is in.
- def current_seg(self):
- if self.prev_seg_pt and self.now_seg_pt:
- if self.theta_version:
- return AngleSegment(self.prev_seg_pt, self.now_seg_pt)
- else:
- return XYSegment(self.prev_seg_pt, self.now_seg_pt)
+ if not self.theta_version:
+ theta1, theta2 = to_theta(self.last_pos, self.circular_index_select)
+ x, y = joint_center[0], joint_center[1]
+ cr.move_to(x, y)
- def do_key_press(self, event):
- print("Gdk.KEY_" + Gdk.keyval_name(event.keyval))
- print("Gdk.KEY_" + Gdk.keyval_name(Gdk.keyval_to_lower(event.keyval)) + " is the lower case key for this button press.")
- if ( Gdk.keyval_to_lower(event.keyval) == Gdk.KEY_q ):
- print("Found q key and exiting.")
- quit_main_loop()
- elif ( Gdk.keyval_to_lower(event.keyval) == Gdk.KEY_c ):
- self.c_i_select += 1
- elif ( Gdk.keyval_to_lower(event.keyval) == Gdk.KEY_v ):
- self.c_i_select -= 1
- elif ( Gdk.keyval_to_lower(event.keyval) == Gdk.KEY_f ):
- self.click_bool = not self.click_bool
+ x += numpy.cos(theta1) * l1
+ y += numpy.sin(theta1) * l1
+ cr.line_to(x, y)
+ x += numpy.cos(theta2) * l2
+ y += numpy.sin(theta2) * l2
+ cr.line_to(x, y)
+ with px(cr):
+ cr.stroke()
- elif ( Gdk.keyval_to_lower(event.keyval) == Gdk.KEY_w ):
- seg = self.current_seg();
- if seg: self.segs.append(seg)
- self.prev_seg_pt = self.now_seg_pt
+ cr.move_to(self.last_pos[0], self.last_pos[1])
+ cr.set_source_rgb(0.0, 1.0, 0.2)
+ draw_px_cross(cr, 20)
- elif ( Gdk.keyval_to_lower(event.keyval) == Gdk.KEY_r ):
- self.prev_seg_pt = self.now_seg_pt
+ if self.theta_version:
+ cr.set_source_rgb(0.0, 1.0, 0.2)
- elif ( Gdk.keyval_to_lower(event.keyval) == Gdk.KEY_p ):
- print(repr(self.segs))
- elif ( Gdk.keyval_to_lower(event.keyval) == Gdk.KEY_g ):
- if self.segs:
- print(repr(self.segs[0].ToThetaPoints()))
- elif ( Gdk.keyval_to_lower(event.keyval) == Gdk.KEY_e ):
- best_pt = self.now_seg_pt
- best_dist = 1e10
- for seg in self.segs:
- d = angle_dist_sqr(seg.st, self.now_seg_pt)
- if (d < best_dist):
- best_pt = seg.st
- best_dist = d;
- d = angle_dist_sqr(seg.ed, self.now_seg_pt)
- if (d < best_dist):
- best_pt = seg.ed
- best_dist = d
- self.now_seg_pt = best_pt
+ cr.set_source_rgb(0.0, 1.0, 0.2)
+ cr.move_to(self.last_pos[0], self.last_pos[1])
+ draw_px_cross(cr, 5)
- elif ( Gdk.keyval_to_lower(event.keyval) == Gdk.KEY_t ):
- if self.theta_version:
- t1, t2 = self.last_pos
- data = to_xy(t1, t2)
- self.c_i_select = int(numpy.floor((t2 - t1) / numpy.pi))
- self.last_pos = (data[0], data[1])
- else:
- self.last_pos = self.cur_pt_in_theta()
+ c_pt, dist = closest_segment(lines_theta, self.last_pos)
+ print("dist:", dist, c_pt, self.last_pos)
+ cr.set_source_rgb(0.0, 1.0, 1.0)
+ cr.move_to(c_pt[0], c_pt[1])
+ draw_px_cross(cr, 5)
- self.theta_version = not self.theta_version
- self.reinit_extents()
- self.redraw()
+ cr.set_source_rgb(0.0, 0.5, 1.0)
+ for segment in self.segments:
+ color = [0, random.random(), 1]
+ random.shuffle(color)
+ cr.set_source_rgb(*color)
+ segment.DrawTo(cr, self.theta_version)
+ with px(cr):
+ cr.stroke()
- def do_button_press(self, event):
- print(event)
- print(event.x, event.y, event.button)
- self.last_pos = (event.x, event.y)
- self.now_seg_pt = self.cur_pt_in_theta();
+ cr.set_source_rgb(0.0, 1.0, 0.5)
+ segment = self.current_seg()
+ if segment:
+ print(segment)
+ segment.DrawTo(cr, self.theta_version)
+ with px(cr):
+ cr.stroke()
- self.redraw()
+ def cur_pt_in_theta(self):
+ if self.theta_version: return self.last_pos
+ return to_theta(self.last_pos, self.circular_index_select)
+
+ # Current segment based on which mode the drawing system is in.
+ def current_seg(self):
+ if self.prev_segment_pt and self.now_segment_pt:
+ if self.theta_version:
+ return AngleSegment(self.prev_segment_pt, self.now_segment_pt)
+ else:
+ return XYSegment(self.prev_segment_pt, self.now_segment_pt)
+
+ def do_key_press(self, event):
+ keyval = Gdk.keyval_to_lower(event.keyval)
+ print("Gdk.KEY_" + Gdk.keyval_name(keyval))
+ if keyval == Gdk.KEY_q:
+ print("Found q key and exiting.")
+ quit_main_loop()
+ elif keyval == Gdk.KEY_c:
+ # Increment which arm solution we render
+ self.circular_index_select += 1
+ print(self.circular_index_select)
+ elif keyval == Gdk.KEY_v:
+ # Decrement which arm solution we render
+ self.circular_index_select -= 1
+ print(self.circular_index_select)
+ elif keyval == Gdk.KEY_w:
+ # Add this segment to the segment list.
+ segment = self.current_seg()
+ if segment: self.segments.append(segment)
+ self.prev_segment_pt = self.now_segment_pt
+
+ elif keyval == Gdk.KEY_r:
+ self.prev_segment_pt = self.now_segment_pt
+
+ elif keyval == Gdk.KEY_p:
+ # Print out the segments.
+ print(repr(self.segments))
+ elif keyval == Gdk.KEY_g:
+ # Generate theta points.
+ if self.segments:
+ print(repr(self.segments[0].ToThetaPoints()))
+ elif keyval == Gdk.KEY_e:
+ best_pt = self.now_segment_pt
+ best_dist = 1e10
+ for segment in self.segments:
+ d = angle_dist_sqr(segment.start, self.now_segment_pt)
+ if (d < best_dist):
+ best_pt = segment.start
+ best_dist = d
+ d = angle_dist_sqr(segment.end, self.now_segment_pt)
+ if (d < best_dist):
+ best_pt = segment.end
+ best_dist = d
+ self.now_segment_pt = best_pt
+
+ elif keyval == Gdk.KEY_t:
+ # Toggle between theta and xy renderings
+ if self.theta_version:
+ theta1, theta2 = self.last_pos
+ data = to_xy(theta1, theta2)
+ self.circular_index_select = int(
+ numpy.floor((theta2 - theta1) / numpy.pi))
+ self.last_pos = (data[0], data[1])
+ else:
+ self.last_pos = self.cur_pt_in_theta()
+
+ self.theta_version = not self.theta_version
+ self.reinit_extents()
+ self.redraw()
+
+ 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]))
+ if not self.theta_version:
+ print('Clicked at xy, circular index: (%f, %f, %f)' %
+ (self.last_pos[0], self.last_pos[1],
+ self.circular_index_select))
+
+ self.redraw()
+
silly = Silly()
-silly.segs = graph_generate.segs
+silly.segments = graph_generate.segments
basic_window.RunApp()
diff --git a/y2018/control_loops/python/graph_generate.py b/y2018/control_loops/python/graph_generate.py
index 232d1a7..034021f 100644
--- a/y2018/control_loops/python/graph_generate.py
+++ b/y2018/control_loops/python/graph_generate.py
@@ -1,204 +1,499 @@
import numpy
# joint_center in x-y space.
-joint_center = (-12.275, 11.775)
+joint_center = (-0.299, 0.299)
# Joint distances (l1 = "proximal", l2 = "distal")
-l1 = 46.25
-l2 = 43.75
+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 c_i mod 2.
-# where c_i is the circular index, or the position in the
+# 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(x, y, orient, cross_point = -numpy.pi):
- x -= joint_center[0]
- y -= joint_center[1]
- l3 = numpy.sqrt(x ** 2 + y ** 2)
- t3 = numpy.arctan2(y, x)
- t1 = numpy.arccos((l1 ** 2 + l3 ** 2 - l2 ** 2) / (2 * l1 * l3))
+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:
- t1 = -t1
- t1 += t3
- t1 = (t1 - cross_point) % (2 * numpy.pi) + cross_point
- t2 = numpy.arctan2(y - l1 * numpy.sin(t1), x - l1 * numpy.cos(t1))
- return (t1, t2)
+ 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(t1, t2):
- x = numpy.cos(t1) * l1 + numpy.cos(t2) * l2 + joint_center[0]
- y = numpy.sin(t1) * l1 + numpy.sin(t2) * l2 + joint_center[1]
- orient = ((t2 - t1) % (2 * numpy.pi)) < numpy.pi
- return (x, y, orient)
+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])
+ cr.move_to(lines[0][0], lines[0][1])
+ for pt in lines[1:]:
+ cr.line_to(pt[0], pt[1])
-max_dist = 1.0
+
+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
+ 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
+ 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
+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
+ 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_ci(x, y, ci):
- t1, t2 = to_theta(x, y, (ci % 2) == 0)
- n_ci = int(numpy.floor((t2 - t1) / numpy.pi))
- t2 = t2 + ((ci - n_ci)) * numpy.pi
- return numpy.array((t1, t2))
+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):
- return b * alpha + (1 - alpha) * a
+ """Blends a and b.
-# Pure vector normalization.
+ 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):
- norm = numpy.linalg.norm(v)
- if norm == 0:
- return v
- return v / norm
+ """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_ci_and_derivs(x, y, dx, dy, c_i_select):
- a = to_theta_with_ci(x, y, c_i_select)
- b = to_theta_with_ci(x + dx * 0.0001, y + dy * 0.0001, c_i_select)
- c = to_theta_with_ci(x - dx * 0.0001, y - dy * 0.0001, 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])
+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)]
+ 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)]
-# subdivision thresholds.
-max_dist = 1.0
-max_dist_theta = numpy.pi / 64
# 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], pt[1], True, 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[0], pt[1], False, cross_point))
- last_pt = n_pt
- return out
+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
+ 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
+ return out
- items = [to_xy(t1, t2) for t1, t2 in lines]
- return [(item[0], item[1]) for item in items]
# Segment in angle space.
class AngleSegment:
- def __init__(self, st, ed):
- self.st = st
- self.ed = ed
- def __repr__(self):
- return "AngleSegment(%s, %s)" % (repr(self.st), repr(self.ed))
+ def __init__(self, start, end, name=None):
+ """Creates an angle segment.
- def DrawTo(self, cr, theta_version):
- if (theta_version):
- cr.move_to(self.st[0], self.st[1])
- cr.line_to(self.ed[0], self.ed[1])
- else:
- draw_lines(cr, back_to_xy_loop([self.st, self.ed]))
+ 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
- def ToThetaPoints(self):
- return [self.st, self.ed]
+ def __repr__(self):
+ return "AngleSegment(%s, %s)" % (repr(self.start), repr(self.end))
-# Segment in X-Y space.
+ 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:
- def __init__(self, st, ed):
- self.st = st
- self.ed = ed
- def __repr__(self):
- return "XYSegment(%s, %s)" % (repr(self.st), repr(self.ed))
- def DrawTo(self, cr, theta_version):
- if (theta_version):
- t1, t2 = self.st
- c_i_select = int(numpy.floor((self.st[1] - self.st[0]) / numpy.pi))
- st = to_xy(*self.st)
- ed = to_xy(*self.ed)
+ """Straight line in XY space."""
- ln = [(st[0], st[1]), (ed[0], ed[1])]
- draw_lines(cr, [to_theta_with_ci(x, y, c_i_select) for x, y in subdivide_xy(ln)])
- else:
- st = to_xy(*self.st)
- ed = to_xy(*self.ed)
- cr.move_to(st[0], st[1])
- cr.line_to(ed[0], ed[1])
+ def __init__(self, start, end, name=None):
+ """Creates an XY segment.
- # Converts to points in theta space via to_theta_with_ci_and_derivs
- def ToThetaPoints(self):
- t1, t2 = self.st
- c_i_select = int(numpy.floor((self.st[1] - self.st[0]) / numpy.pi))
- st = to_xy(*self.st)
- ed = to_xy(*self.ed)
+ 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
- ln = [(st[0], st[1]), (ed[0], ed[1])]
+ def __repr__(self):
+ return "XYSegment(%s, %s)" % (repr(self.start), repr(self.end))
- dx = ed[0] - st[0]
- dy = ed[1] - st[1]
- mag = numpy.sqrt((dx) ** 2 + (dy) ** 2)
- dx /= mag
- dy /= mag
+ 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)
- return [to_theta_with_ci_and_derivs(x, y, dx, dy, c_i_select) for x, y in subdivide_xy(ln, 1.0)]
+ 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)
-segs = [XYSegment((1.3583511559969876, 0.99753029519739866), (0.97145546090878643, -1.4797428713062153))]
-segs = [XYSegment((1.3583511559969876, 0.9975302951973987), (1.5666193247337956, 0.042054827580659759))]
+ 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):
+ self.start = start
+ self.control1 = control1
+ self.control2 = control2
+ self.end = end
+ self.name = name
+
+ def __repr__(self):
+ return "XYSegment(%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)
+ ])
+ else:
+ start = get_xy(self.start)
+ control1 = get_xy(self.control1)
+ control2 = get_xy(self.control2)
+ end = get_xy(self.end)
+ #cr.move_to(start[0], start[1])
+ draw_lines(cr, [
+ spline_eval(start, control1, control2, end, alpha)
+ for alpha in subdivide_spline(start, control1, control2, end)
+ ])
+ # cr.spline_to(control1[0], control1[1], control2[0], control2[1], end[0], end[1])
+
+ 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)
+ ]
+
+
+tall_box_x = 0.401
+tall_box_y = 0.13
+
+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_middle2_box = to_theta_with_circular_index(
+ 0.732, 2.268, circular_index=-1)
+front_middle1_box = to_theta_with_circular_index(
+ 0.878, 1.885, circular_index=-1)
+front_low_box = to_theta_with_circular_index(0.926, 1.522, 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.732, 2.268, circular_index=0)
+back_middle1_box = to_theta_with_circular_index(
+ -0.878, 1.885, circular_index=0)
+back_low_box = to_theta_with_circular_index(-0.926, 1.522, 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)
+
+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)
+
+ready_above_box_c1 = to_theta((0.38, 0.33), circular_index=-1)
+ready_above_box_c2 = to_theta((0.42, 0.51), circular_index=-1)
+
+points = [(ready_above_box, "ReadyAboveBox"),
+ (tall_box_grab, "TallBoxGrab"),
+ (short_box_grab, "ShortBoxGrab"),
+ (front_high_box, "FrontHighBox"),
+ (front_middle2_box, "FrontMiddle2Box"),
+ (front_middle1_box, "FrontMiddle1Box"),
+ (front_low_box, "FrontLowBox"),
+ (back_high_box, "BackHighBox"),
+ (back_middle2_box, "BackMiddle2Box"),
+ (back_middle1_box, "BackMiddle1Box"),
+ (back_low_box, "BackLowBox"),
+ (front_switch, "FrontSwitch"),
+ (back_switch, "BackSwitch"),
+ (neutral, "Neutral"),
+ (up, "Up")] # yapf: disable
+
+# We need to define critical points so we can create paths connecting them.
+# TODO(austin): Attach velocities to the slow ones.
+named_segments = [
+ XYSegment(ready_above_box, tall_box_grab, "ReadyToTallBox"),
+ XYSegment(ready_above_box, short_box_grab, "ReadyToShortBox"),
+ XYSegment(tall_box_grab, short_box_grab, "TallToShortBox"),
+ SplineSegment(neutral, ready_above_box_c1, ready_above_box_c2,
+ ready_above_box, "ReadyToNeutral"),
+ 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, "NeutralToFronMiddle2"),
+ SplineSegment(neutral, front_middle1_box_c1, front_middle1_box_c2,
+ front_middle1_box, "NeutralToFronMiddle1"),
+]
+
+unnamed_segments = [
+ AngleSegment(neutral, back_switch),
+ XYSegment(neutral, front_switch),
+
+ XYSegment(up, front_high_box),
+ XYSegment(up, front_middle2_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),
+ 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),
+]
+
+segments = named_segments + unnamed_segments