diff --git a/y2023/control_loops/python/BUILD b/y2023/control_loops/python/BUILD
index 6109255..9a5a156 100644
--- a/y2023/control_loops/python/BUILD
+++ b/y2023/control_loops/python/BUILD
@@ -44,6 +44,7 @@
         ":python_init",
         "//frc971/control_loops/python:basic_window",
         "//frc971/control_loops/python:color",
+        "@pip//matplotlib",
         "@pip//numpy",
         "@pip//pygobject",
         "@pip//shapely",
@@ -55,6 +56,7 @@
     srcs = [
         "graph_codegen.py",
         "graph_paths.py",
+        "graph_tools.py",
     ],
     legacy_create_init = False,
     target_compatible_with = ["@platforms//os:linux"],
diff --git a/y2023/control_loops/python/graph_codegen.py b/y2023/control_loops/python/graph_codegen.py
index 667a78d..54b77a9 100644
--- a/y2023/control_loops/python/graph_codegen.py
+++ b/y2023/control_loops/python/graph_codegen.py
@@ -1,7 +1,7 @@
 from __future__ import print_function
 import sys
-import numpy
-import graph_paths
+import numpy as np
+import y2023.control_loops.python.graph_paths as graph_paths
 
 
 def index_function_name(name):
@@ -20,26 +20,36 @@
 
     alpha_unitizer = "alpha_unitizer"
     if segment.alpha_unitizer is not None:
-        alpha_unitizer = "(::Eigen::Matrix<double, 2, 2>() << %f, %f, %f, %f).finished()" % (
-            segment.alpha_unitizer[0, 0], segment.alpha_unitizer[0, 1],
-            segment.alpha_unitizer[1, 0], segment.alpha_unitizer[1, 1])
+        alpha_unitizer = "(::Eigen::Matrix<double, 3, 3>() << %f, %f, %f, %f, %f, %f, %f, %f, %f).finished()" % (
+            segment.alpha_unitizer[0, 0],
+            segment.alpha_unitizer[0, 1],
+            segment.alpha_unitizer[0, 2],
+            segment.alpha_unitizer[1, 0],
+            segment.alpha_unitizer[1, 1],
+            segment.alpha_unitizer[1, 2],
+            segment.alpha_unitizer[2, 0],
+            segment.alpha_unitizer[2, 1],
+            segment.alpha_unitizer[2, 2],
+        )
     cc_file.append("  trajectories->emplace_back(%s," % (vmax))
     cc_file.append("                             %s," % (alpha_unitizer))
     if reverse:
         cc_file.append(
-            "                             Trajectory(dynamics, Path::Reversed(%s()), 0.005));"
+            "                             Trajectory(dynamics, &hybrid_roll_joint_loop->plant(), Path::Reversed(%s()), 0.005));"
             % (path_function_name(str(name))))
     else:
         cc_file.append(
-            "                             Trajectory(dynamics, %s(), 0.005));"
+            "                             Trajectory(dynamics, &hybrid_roll_joint_loop->plant(), %s(), 0.005));"
             % (path_function_name(str(name))))
 
     start_index = None
     end_index = None
     for point, name in graph_paths.points:
-        if (point == segment.start).all():
+        if (point[:2] == segment.start
+            ).all() and point[2] == segment.alpha_rolls[0][1]:
             start_index = name
-        if (point == segment.end).all():
+        if (point[:2] == segment.end
+            ).all() and point[2] == segment.alpha_rolls[-1][1]:
             end_index = name
 
     if reverse:
@@ -61,20 +71,27 @@
 
 def main(argv):
     cc_file = []
-    cc_file.append(
-        "#include \"y2023/control_loops/superstructure/arm/generated_graph.h\""
-    )
     cc_file.append("")
     cc_file.append("#include <memory>")
     cc_file.append("")
     cc_file.append(
-        "#include \"frc971/control_loops/double_jointed_arm/trajectory.h\"")
-    cc_file.append(
         "#include \"frc971/control_loops/double_jointed_arm/graph.h\"")
+    cc_file.append(
+        "#include \"y2023/control_loops/superstructure/arm/generated_graph.h\""
+    )
+    cc_file.append(
+        "#include \"y2023/control_loops/superstructure/arm/trajectory.h\"")
+    cc_file.append(
+        "#include \"y2023/control_loops/superstructure/roll/integral_hybrid_roll_plant.h\""
+    )
 
-    cc_file.append("using frc971::control_loops::arm::Trajectory;")
-    cc_file.append("using frc971::control_loops::arm::Path;")
     cc_file.append("using frc971::control_loops::arm::SearchGraph;")
+    cc_file.append(
+        "using y2023::control_loops::superstructure::arm::Trajectory;")
+    cc_file.append("using y2023::control_loops::superstructure::arm::Path;")
+    cc_file.append("using y2023::control_loops::superstructure::arm::NSpline;")
+    cc_file.append(
+        "using y2023::control_loops::superstructure::arm::CosSpline;")
 
     cc_file.append("")
     cc_file.append("namespace y2023 {")
@@ -91,35 +108,37 @@
     h_file.append("#include <memory>")
     h_file.append("")
     h_file.append(
-        "#include \"frc971/control_loops/double_jointed_arm/trajectory.h\"")
-    h_file.append(
         "#include \"frc971/control_loops/double_jointed_arm/graph.h\"")
     h_file.append(
         "#include \"y2023/control_loops/superstructure/arm/arm_constants.h\"")
+    h_file.append(
+        "#include \"y2023/control_loops/superstructure/arm/trajectory.h\"")
+
     h_file.append("")
     h_file.append("namespace y2023 {")
     h_file.append("namespace control_loops {")
     h_file.append("namespace superstructure {")
     h_file.append("namespace arm {")
 
-    h_file.append("using frc971::control_loops::arm::Trajectory;")
-    h_file.append("using frc971::control_loops::arm::Path;")
     h_file.append("using frc971::control_loops::arm::SearchGraph;")
     h_file.append(
+        "using y2023::control_loops::superstructure::arm::Trajectory;")
+    h_file.append("using y2023::control_loops::superstructure::arm::Path;")
+    h_file.append(
         "using y2023::control_loops::superstructure::arm::kArmConstants;")
 
     h_file.append("")
     h_file.append("struct TrajectoryAndParams {")
     h_file.append("  TrajectoryAndParams(double new_vmax,")
     h_file.append(
-        "                      const ::Eigen::Matrix<double, 2, 2> &new_alpha_unitizer,"
+        "                      const ::Eigen::Matrix<double, 3, 3> &new_alpha_unitizer,"
     )
     h_file.append("                      Trajectory &&new_trajectory)")
     h_file.append("      : vmax(new_vmax),")
     h_file.append("        alpha_unitizer(new_alpha_unitizer),")
     h_file.append("        trajectory(::std::move(new_trajectory)) {}")
     h_file.append("  double vmax;")
-    h_file.append("  ::Eigen::Matrix<double, 2, 2> alpha_unitizer;")
+    h_file.append("  ::Eigen::Matrix<double, 3, 3> alpha_unitizer;")
     h_file.append("  Trajectory trajectory;")
     h_file.append("};")
     h_file.append("")
@@ -129,11 +148,12 @@
         h_file.append("")
         h_file.append("constexpr uint32_t %s() { return %d; }" %
                       (index_function_name(point[1]), index))
-        h_file.append("inline ::Eigen::Matrix<double, 2, 1> %sPoint() {" %
+        h_file.append("inline ::Eigen::Matrix<double, 3, 1> %sPoint() {" %
                       point[1])
         h_file.append(
-            "  return (::Eigen::Matrix<double, 2, 1>() << %f, %f).finished();"
-            % (numpy.pi / 2.0 - point[0][0], numpy.pi / 2.0 - point[0][1]))
+            "  return (::Eigen::Matrix<double, 3, 1>() << %f, %f, %f).finished();"
+            % (np.pi / 2.0 - point[0][0], np.pi / 2.0 - point[0][1],
+               np.pi / 2.0 - point[0][2]))
         h_file.append("}")
 
     front_points = [
@@ -168,26 +188,39 @@
                       path_function_name(name))
         cc_file.append("::std::unique_ptr<Path> %s() {" %
                        path_function_name(name))
-        cc_file.append("  return ::std::unique_ptr<Path>(new Path({")
-        for point in segment.ToThetaPoints():
-            cc_file.append("      {{%.12f, %.12f, %.12f," %
-                           (numpy.pi / 2.0 - point[0],
-                            numpy.pi / 2.0 - point[1], -point[2]))
-            cc_file.append("        %.12f, %.12f, %.12f}}," %
-                           (-point[3], -point[4], -point[5]))
-        cc_file.append("  }));")
+        cc_file.append(
+            "  return ::std::unique_ptr<Path>(new Path(CosSpline{NSpline<4, 2>((Eigen::Matrix<double, 2, 4>() << "
+        )
+        points = [
+            segment.start, segment.control1, segment.control2, segment.end
+        ]
+        for i in range(len(points)):
+            cc_file.append("%.12f," % (np.pi / 2.0 - points[i][0]))
+        for i in range(len(points)):
+            cc_file.append("%.12f%s" % (np.pi / 2.0 - points[i][1],
+                                        ", " if i != len(points) - 1 else ""))
+
+        cc_file.append(").finished()), {")
+        for alpha, roll in segment.alpha_rolls:
+            cc_file.append(
+                "CosSpline::AlphaTheta{.alpha = %.12f, .theta = %.12f}" %
+                (alpha, np.pi / 2.0 - roll))
+            if alpha != segment.alpha_rolls[-1][0]:
+                cc_file.append(", ")
+        cc_file.append("  }}));")
         cc_file.append("}")
 
     # Matrix of nodes
-    h_file.append("::std::vector<::Eigen::Matrix<double, 2, 1>> PointList();")
+    h_file.append("::std::vector<::Eigen::Matrix<double, 3, 1>> PointList();")
 
     cc_file.append(
-        "::std::vector<::Eigen::Matrix<double, 2, 1>> PointList() {")
-    cc_file.append("  ::std::vector<::Eigen::Matrix<double, 2, 1>> points;")
+        "::std::vector<::Eigen::Matrix<double, 3, 1>> PointList() {")
+    cc_file.append("  ::std::vector<::Eigen::Matrix<double, 3, 1>> points;")
     for point in graph_paths.points:
         cc_file.append(
-            "  points.push_back((::Eigen::Matrix<double, 2, 1>() << %.12s, %.12s).finished());"
-            % (numpy.pi / 2.0 - point[0][0], numpy.pi / 2.0 - point[0][1]))
+            "  points.push_back((::Eigen::Matrix<double, 3, 1>() << %.12s, %.12s, %.12s).finished());"
+            % (np.pi / 2.0 - point[0][0], np.pi / 2.0 - point[0][1],
+               np.pi / 2.0 - point[0][2]))
     cc_file.append("  return points;")
     cc_file.append("}")
 
@@ -198,15 +231,22 @@
                   "const frc971::control_loops::arm::Dynamics *dynamics, "
                   "::std::vector<TrajectoryAndParams> *trajectories,")
     h_file.append("                            "
-                  "const ::Eigen::Matrix<double, 2, 2> &alpha_unitizer,")
-    h_file.append("                            double vmax);")
+                  "const ::Eigen::Matrix<double, 3, 3> &alpha_unitizer,")
+    h_file.append("                            "
+                  "double vmax,")
+    h_file.append(
+        "const StateFeedbackLoop<3, 1, 1, double, StateFeedbackHybridPlant<3, 1, 1>, HybridKalman<3, 1, 1>> *hybrid_roll_joint_loop);"
+    )
     cc_file.append("SearchGraph MakeSearchGraph("
                    "const frc971::control_loops::arm::Dynamics *dynamics, "
                    "::std::vector<TrajectoryAndParams> *trajectories,")
     cc_file.append("                            "
-                   "const ::Eigen::Matrix<double, 2, 2> &alpha_unitizer,")
+                   "const ::Eigen::Matrix<double, 3, 3> &alpha_unitizer,")
     cc_file.append("                            "
-                   "double vmax) {")
+                   "double vmax,")
+    cc_file.append(
+        "const StateFeedbackLoop<3, 1, 1, double, StateFeedbackHybridPlant<3, 1, 1>, HybridKalman<3, 1, 1>> *hybrid_roll_joint_loop) {"
+    )
     cc_file.append("  ::std::vector<SearchGraph::Edge> edges;")
 
     index = 0
diff --git a/y2023/control_loops/python/graph_edit.py b/y2023/control_loops/python/graph_edit.py
index 3ef9d07..93043b7 100644
--- a/y2023/control_loops/python/graph_edit.py
+++ b/y2023/control_loops/python/graph_edit.py
@@ -6,14 +6,14 @@
 from frc971.control_loops.python.color import Color, palette
 import random
 import gi
-import numpy
+import numpy as np
 
 gi.require_version('Gtk', '3.0')
 from gi.repository import Gdk, Gtk
 import cairo
-from graph_tools import XYSegment, AngleSegment, to_theta, to_xy, alpha_blend
-from graph_tools import back_to_xy_loop, subdivide_theta, to_theta_loop
+from graph_tools import to_theta, to_xy, alpha_blend
 from graph_tools import l1, l2, joint_center
+from graph_tools import DRIVER_CAM_POINTS
 import graph_paths
 
 from frc971.control_loops.python.basic_window import quit_main_loop, set_color, OverrideMatrix, identity
@@ -21,6 +21,8 @@
 import shapely
 from shapely.geometry import Polygon
 
+import matplotlib.pyplot as plt
+
 
 def px(cr):
     return OverrideMatrix(cr, identity)
@@ -55,39 +57,13 @@
 
 # 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]
-
-
-# This is the x position where the inner (hyperextension) circle intersects the horizontal line
-derr = numpy.sqrt((l1 - l2)**2 - (joint_center[1] - 0.3048)**2)
+    return np.sqrt((l2 + l1)**2 - (x - joint_center[0])**2) + joint_center[1]
 
 
 # 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])
-
-
-# left hand side lines
-lines1 = [
-    (-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, 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) * 0.0254)
-t2_min = -7.0 / 4.0 * numpy.pi
-
-t1_max = get_angle((-32.525 + 4.0) * 0.0254)
-t2_max = numpy.pi * 3.0 / 4.0
+    h = np.sqrt((l1)**2 - (boundary - joint_center[0])**2) + joint_center[1]
+    return np.arctan2(h, boundary - joint_center[0])
 
 
 # Rotate a rasterized loop such that it aligns to when the parameters loop
@@ -96,7 +72,7 @@
     for pt_i in range(1, len(points)):
         pt = points[pt_i]
         delta = last_pt[1] - pt[1]
-        if abs(delta) > numpy.pi:
+        if abs(delta) > np.pi:
             return points[pt_i:] + points[:pt_i]
         last_pt = pt
     return points
@@ -107,39 +83,14 @@
     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))
-
-# Some hacks here to make a single polygon by shifting to get an extra copy of the contraints.
-lines1_theta = y_shift(lines1_theta_part, -numpy.pi * 2) + lines1_theta_part + \
-    y_shift(lines1_theta_part, numpy.pi * 2)
-lines2_theta = y_shift(lines2_theta_part, numpy.pi * 2) + lines2_theta_part + \
-    y_shift(lines2_theta_part, -numpy.pi * 2)
-
-lines_theta = lines1_theta + lines2_theta
-
-p1 = Polygon(lines_theta)
-
-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)
-
-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])
 
-    d = numpy.sqrt(dx_ang**2 + dy_ang**2)
+    d = np.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)
+        return prev, np.sqrt((prev[0] - pt[0])**2 + (prev[1] - pt[1])**2)
 
     pdx = -dy_ang / d
     pdy = dx_ang / d
@@ -150,9 +101,9 @@
     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)
+        return prev, np.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)
+        return cur, np.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)
@@ -171,10 +122,10 @@
 
 
 # Create a GTK+ widget on which we will draw using Cairo
-class Silly(basic_window.BaseWindow):
+class ArmUi(basic_window.BaseWindow):
 
     def __init__(self):
-        super(Silly, self).__init__()
+        super(ArmUi, self).__init__()
 
         self.window = Gtk.Window()
         self.window.set_title("DrawingArea")
@@ -185,6 +136,7 @@
                                | Gdk.EventMask.SCROLL_MASK
                                | Gdk.EventMask.KEY_PRESS_MASK)
         self.method_connect("key-press-event", self.do_key_press)
+        self.method_connect("motion-notify-event", self.do_motion)
         self.method_connect("button-press-event",
                             self._do_button_press_internal)
         self.method_connect("configure-event", self._do_configure)
@@ -194,7 +146,7 @@
         self.theta_version = False
         self.reinit_extents()
 
-        self.last_pos = (numpy.pi / 2.0, 1.0)
+        self.last_pos = (np.pi / 2.0, 1.0)
         self.circular_index_select = -1
 
         # Extra stuff for drawing lines.
@@ -204,6 +156,12 @@
         self.spline_edit = 0
         self.edit_control1 = True
 
+        self.roll_joint_thetas = None
+        self.roll_joint_point = None
+        self.fig = plt.figure()
+        self.ax = self.fig.add_subplot(111)
+        plt.show(block=False)
+
     def do_key_press(self, event):
         pass
 
@@ -236,10 +194,10 @@
 
     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
+            self.extents_x_min = -np.pi * 2
+            self.extents_x_max = np.pi * 2
+            self.extents_y_min = -np.pi * 2
+            self.extents_y_max = np.pi * 2
         else:
             self.extents_x_min = -40.0 * 0.0254
             self.extents_x_max = 40.0 * 0.0254
@@ -270,91 +228,75 @@
         if self.theta_version:
             # Draw a filled white rectangle.
             set_color(cr, palette["WHITE"])
-            cr.rectangle(-numpy.pi, -numpy.pi, numpy.pi * 2.0, numpy.pi * 2.0)
+            cr.rectangle(-np.pi, -np.pi, np.pi * 2.0, np.pi * 2.0)
             cr.fill()
 
             set_color(cr, palette["BLUE"])
             for i in range(-6, 6):
-                cr.move_to(-40, -40 + i * numpy.pi)
-                cr.line_to(40, 40 + i * numpy.pi)
+                cr.move_to(-40, -40 + i * np.pi)
+                cr.line_to(40, 40 + i * np.pi)
             with px(cr):
                 cr.stroke()
 
-            set_color(cr, Color(0.5, 0.5, 1.0))
-            draw_lines(cr, lines_theta)
-
             set_color(cr, Color(0.0, 1.0, 0.2))
             cr.move_to(self.last_pos[0], self.last_pos[1])
             draw_px_cross(cr, 5)
 
-            c_pt, dist = closest_segment(lines_theta, self.last_pos)
-            print("dist:", dist, c_pt, self.last_pos)
-            set_color(cr, palette["CYAN"])
-            cr.move_to(c_pt[0], c_pt[1])
-            draw_px_cross(cr, 5)
         else:
             # Draw a filled white rectangle.
             set_color(cr, palette["WHITE"])
             cr.rectangle(-2.0, -2.0, 4.0, 4.0)
             cr.fill()
 
+            # Draw top of drivetrain (including bumpers)
+            DRIVETRAIN_X = -0.490
+            DRIVETRAIN_Y = 0.184
+            DRIVETRAIN_WIDTH = 0.980
             set_color(cr, palette["BLUE"])
-            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)
+            cr.move_to(DRIVETRAIN_X, DRIVETRAIN_Y)
+            cr.line_to(DRIVETRAIN_X + DRIVETRAIN_WIDTH, DRIVETRAIN_Y)
             with px(cr):
                 cr.stroke()
 
-            set_color(cr, Color(0.5, 1.0, 1.0))
-            draw_lines(cr, lines1)
-            draw_lines(cr, lines2)
+            # Draw joint center
+            JOINT_CENTER_RADIUS = 0.173 / 2
+            cr.arc(joint_center[0], joint_center[1], JOINT_CENTER_RADIUS, 0,
+                   2.0 * np.pi)
+            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
+            JOINT_TOWER_X = -0.252
+            JOINT_TOWER_Y = DRIVETRAIN_Y
+            JOINT_TOWER_WIDTH = 0.098
+            JOINT_TOWER_HEIGHT = 0.864
+            cr.rectangle(JOINT_TOWER_X, JOINT_TOWER_Y, JOINT_TOWER_WIDTH,
+                         JOINT_TOWER_HEIGHT)
+            with px(cr):
+                cr.stroke()
 
+            # Draw driver cam
+            cr.set_source_rgba(1, 0, 0, 0.5)
+            DRIVER_CAM_X = DRIVER_CAM_POINTS[0][0]
+            DRIVER_CAM_Y = DRIVER_CAM_POINTS[0][1]
+            DRIVER_CAM_WIDTH = DRIVER_CAM_POINTS[-1][0] - DRIVER_CAM_POINTS[0][
+                0]
+            DRIVER_CAM_HEIGHT = DRIVER_CAM_POINTS[-1][1] - DRIVER_CAM_POINTS[
+                0][1]
+            cr.rectangle(DRIVER_CAM_X, DRIVER_CAM_Y, DRIVER_CAM_WIDTH,
+                         DRIVER_CAM_HEIGHT)
+            with px(cr):
+                cr.fill()
+
+            # Draw max radius
             set_color(cr, palette["BLUE"])
-            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])
-
+            cr.arc(joint_center[0], joint_center[1], l2 + l1, 0, 2.0 * np.pi)
+            with px(cr):
+                cr.stroke()
+            cr.arc(joint_center[0], joint_center[1], l1 - l2, 0, 2.0 * np.pi)
             with px(cr):
                 cr.stroke()
 
-            theta1, theta2 = to_theta(self.last_pos,
-                                      self.circular_index_select)
-            x, y = joint_center[0], joint_center[1]
-            cr.move_to(x, y)
-
-            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()
-
-            cr.move_to(self.last_pos[0], self.last_pos[1])
-            set_color(cr, Color(0.0, 1.0, 0.2))
-            draw_px_cross(cr, 20)
+            set_color(cr, Color(0.5, 1.0, 1))
 
         set_color(cr, Color(0.0, 0.5, 1.0))
         for segment in self.segments:
@@ -366,25 +308,57 @@
                 cr.stroke()
 
         set_color(cr, Color(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()
+
+        # Create the roll joint plot
+        if self.roll_joint_thetas:
+            self.ax.clear()
+            self.ax.plot(*self.roll_joint_thetas)
+            if self.roll_joint_point:
+                self.ax.scatter([self.roll_joint_point[0]],
+                                [self.roll_joint_point[1]],
+                                s=10,
+                                c="red")
+            plt.title("Roll Joint Angle")
+            plt.xlabel("t (0 to 1)")
+            plt.ylabel("theta (rad)")
+
+            self.fig.canvas.draw()
 
     def cur_pt_in_theta(self):
-        if self.theta_version: return numpy.asarray(self.last_pos)
+        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 is not None and (self.prev_segment_pt.any() and
-                                                 self.now_segment_pt.any()):
-            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_motion(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]
+
+        for segment in self.segments:
+            self.roll_joint_thetas = segment.roll_joint_thetas()
+
+            hovered_t = segment.intersection(event)
+            if hovered_t:
+                min_diff = np.inf
+                closest_t = None
+                closest_theta = None
+                for i in range(len(self.roll_joint_thetas[0])):
+                    t = self.roll_joint_thetas[0][i]
+                    diff = abs(t - hovered_t)
+                    if diff < min_diff:
+                        min_diff = diff
+                        closest_t = t
+                        closest_theta = self.roll_joint_thetas[1][i]
+                self.roll_joint_point = (closest_t, closest_theta)
+                break
+
+        event.x = o_x
+        event.y = o_y
+
+        self.redraw()
 
     def do_key_press(self, event):
         keyval = Gdk.keyval_to_lower(event.keyval)
@@ -400,11 +374,6 @@
             # 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
@@ -436,7 +405,7 @@
                 theta1, theta2 = self.last_pos
                 data = to_xy(theta1, theta2)
                 self.circular_index_select = int(
-                    numpy.floor((theta2 - theta1) / numpy.pi))
+                    np.floor((theta2 - theta1) / np.pi))
                 self.last_pos = (data[0], data[1])
             else:
                 self.last_pos = self.cur_pt_in_theta()
@@ -476,14 +445,14 @@
                   (self.last_pos[0], self.last_pos[1],
                    self.circular_index_select))
 
-        print('c1: numpy.array([%f, %f])' %
+        print('c1: np.array([%f, %f])' %
               (self.segments[0].control1[0], self.segments[0].control1[1]))
-        print('c2: numpy.array([%f, %f])' %
+        print('c2: np.array([%f, %f])' %
               (self.segments[0].control2[0], self.segments[0].control2[1]))
 
         self.redraw()
 
 
-silly = Silly()
-silly.segments = graph_paths.segments
+arm_ui = ArmUi()
+arm_ui.segments = graph_paths.segments
 basic_window.RunApp()
diff --git a/y2023/control_loops/python/graph_paths.py b/y2023/control_loops/python/graph_paths.py
index cfc68ac..c60a2c5 100644
--- a/y2023/control_loops/python/graph_paths.py
+++ b/y2023/control_loops/python/graph_paths.py
@@ -1,37 +1,41 @@
-import numpy
+import numpy as np
 
-from graph_tools import *
+from y2023.control_loops.python.graph_tools import *
 
-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)
+neutral = to_theta_with_circular_index_and_roll(joint_center[0],
+                                                joint_center[1] + l2 - l1,
+                                                np.pi / 2,
+                                                circular_index=-1)
 
-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)
+neutral_to_pickup_1 = to_theta_with_circular_index(0.3, 0.6, circular_index=-1)
+neutral_to_pickup_2 = to_theta_with_circular_index(0.3, 0.4, circular_index=-1)
+pickup_pos = to_theta_with_circular_index_and_roll(0.6,
+                                                   0.1,
+                                                   np.pi / 2,
+                                                   circular_index=-1)
+neutral_to_pickup_control_alpha_rolls = [(0.33, np.pi / 2), (.67, np.pi / 2)]
 
-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)
+neutral_to_score_1 = to_theta_with_circular_index(-0.4, 1.2, circular_index=-1)
+neutral_to_score_2 = to_theta_with_circular_index(-0.7, 1.2, circular_index=-1)
+score_pos = to_theta_with_circular_index_and_roll(-1.0,
+                                                  1.2,
+                                                  np.pi / 2,
+                                                  circular_index=-1)
+neutral_to_score_control_alpha_rolls = [(0.33, np.pi / 2), (.67, np.pi / 2)]
 
 # TODO(Max): Add real paths for arm.
-points = [(neutral, "NeutralPos"), (neutral_to_cone_1, "NeutralToConePos1"),
-          (neutral_to_cone_2, "NeutralToConePos2"), (cone_pos, "ConePos"),
-          (neutral_to_cone_perch_pos_1, "NeutralToConePerchPos1"),
-          (neutral_to_cone_perch_pos_2, "NeutralToConePerchPos2"),
-          (cone_perch_pos, "ConePerchPos")]
+points = [(neutral, "NeutralPos"), (pickup_pos, "PickupPos"),
+          (score_pos, "ScorePos")]
 front_points = []
 back_points = []
 unnamed_segments = []
 named_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,
-                       "NeutralToConePerch"),
+    ThetaSplineSegment("NeutralToPickup", neutral, neutral_to_pickup_1,
+                       neutral_to_pickup_2, pickup_pos,
+                       neutral_to_pickup_control_alpha_rolls),
+    ThetaSplineSegment("NeutralToScore", neutral, neutral_to_score_1,
+                       neutral_to_score_2, score_pos,
+                       neutral_to_score_control_alpha_rolls),
 ]
 
-segments = unnamed_segments + named_segments
+segments = named_segments + unnamed_segments
diff --git a/y2023/control_loops/python/graph_tools.py b/y2023/control_loops/python/graph_tools.py
index dafa294..3b5048e 100644
--- a/y2023/control_loops/python/graph_tools.py
+++ b/y2023/control_loops/python/graph_tools.py
@@ -1,14 +1,18 @@
-import numpy
+import abc
+import numpy as np
+import sys
+import traceback
 
 # joint_center in x-y space.
-joint_center = (-0.299, 0.299)
+IN_TO_M = 0.0254
+joint_center = (-0.203, 0.787)
 
 # Joint distances (l1 = "proximal", l2 = "distal")
-l1 = 46.25 * 0.0254
-l2 = 43.75 * 0.0254
+l1 = 20.0 * IN_TO_M
+l2 = 31.5 * IN_TO_M
 
 max_dist = 0.01
-max_dist_theta = numpy.pi / 64
+max_dist_theta = np.pi / 64
 xy_end_circle_size = 0.01
 theta_end_circle_size = 0.07
 
@@ -18,43 +22,170 @@
 # 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):
+def to_theta(pt, circular_index, cross_point=-np.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))
+    l3 = np.hypot(x, y)
+    t3 = np.arctan2(y, x)
+    theta1 = np.arccos((l1**2 + l3**2 - l2**2) / (2 * l1 * l3))
+    if np.isnan(theta1):
+        traceback.print_stack()
+        sys.exit("Couldn't fit triangle to %f, %f, %f" % (l1, l2, 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))
+    theta1 = (theta1 - cross_point) % (2 * np.pi) + cross_point
+    theta2 = np.arctan2(y - l1 * np.sin(theta1), x - l1 * np.cos(theta1))
+    return np.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
+    x = np.cos(theta1) * l1 + np.cos(theta2) * l2 + joint_center[0]
+    y = np.sin(theta1) * l1 + np.sin(theta2) * l2 + joint_center[1]
+    orient = ((theta2 - theta1) % (2.0 * np.pi)) < np.pi
     return (x, y, orient)
 
 
+END_EFFECTOR_X_LEN = (-1.0 * IN_TO_M, 10.425 * IN_TO_M)
+END_EFFECTOR_Y_LEN = (-4.875 * IN_TO_M, 7.325 * IN_TO_M)
+END_EFFECTOR_Z_LEN = (-11.0 * IN_TO_M, 11.0 * IN_TO_M)
+
+
+def abs_sum(l):
+    result = 0
+    for e in l:
+        result += abs(e)
+    return result
+
+
+def affine_3d(R, T):
+    H = np.eye(4)
+    H[:3, 3] = T
+    H[:3, :3] = R
+    return H
+
+
+# Simple trig to go back from theta1, theta2, and theta3 to
+# the 8 corners on the roll joint x-y-z
+def to_end_effector_points(theta1, theta2, theta3):
+    x, y, _ = to_xy(theta1, theta2)
+    # Homogeneous end effector points relative to the end_effector
+    # ee = end effector
+    endpoints_ee = []
+    for i in range(2):
+        for j in range(2):
+            for k in range(2):
+                endpoints_ee.append(
+                    np.array((END_EFFECTOR_X_LEN[i], END_EFFECTOR_Y_LEN[j],
+                              END_EFFECTOR_Z_LEN[k], 1.0)))
+
+    # Only roll.
+    # rj = roll joint
+    roll = theta3
+    T_rj_ee = np.zeros(3)
+    R_rj_ee = np.array([[1.0, 0.0, 0.0], [0.0,
+                                          np.cos(roll), -np.sin(roll)],
+                        [0.0, np.sin(roll), np.cos(roll)]])
+    H_rj_ee = affine_3d(R_rj_ee, T_rj_ee)
+
+    # Roll joint pose relative to the origin
+    # o = origin
+    T_o_rj = np.array((x, y, 0))
+    # Only yaw
+    yaw = theta1 + theta2
+    R_o_rj = [[np.cos(yaw), -np.sin(yaw), 0.0],
+              [np.sin(yaw), np.cos(yaw), 0.0], [0.0, 0.0, 1.0]]
+    H_o_rj = affine_3d(R_o_rj, T_o_rj)
+
+    # Now compute the pose of the end effector relative to the origin
+    H_o_ee = H_o_rj @ H_rj_ee
+
+    # Get the translation from these transforms
+    endpoints_o = [(H_o_ee @ endpoint_ee)[:3] for endpoint_ee in endpoints_ee]
+
+    diagonal_distance = np.linalg.norm(
+        np.array(endpoints_o[0]) - np.array(endpoints_o[-1]))
+    actual_diagonal_distance = np.linalg.norm(
+        np.array((abs_sum(END_EFFECTOR_X_LEN), abs_sum(END_EFFECTOR_Y_LEN),
+                  abs_sum(END_EFFECTOR_Z_LEN))))
+    assert abs(diagonal_distance - actual_diagonal_distance) < 1e-5
+
+    return np.array(endpoints_o)
+
+
+# Returns all permutations of rectangle points given two opposite corners.
+# x is the two x values, y is the two y values, z is the two z values
+def rect_points(x, y, z):
+    points = []
+    for i in range(2):
+        for j in range(2):
+            for k in range(2):
+                points.append((x[i], y[j], z[k]))
+    return np.array(points)
+
+
+DRIVER_CAM_Z_OFFSET = 3.225 * IN_TO_M
+DRIVER_CAM_POINTS = rect_points(
+    (-5.126 * IN_TO_M + joint_center[0], 0.393 * IN_TO_M + joint_center[0]),
+    (5.125 * IN_TO_M + joint_center[1], 17.375 * IN_TO_M + joint_center[1]),
+    (-8.475 * IN_TO_M - DRIVER_CAM_Z_OFFSET,
+     -4.350 * IN_TO_M - DRIVER_CAM_Z_OFFSET))
+
+
+def compute_face_normals(points):
+    # Return the normal vectors of all the faces
+    normals = []
+    for i in range(points.shape[0]):
+        v1 = points[i]
+        v2 = points[(i + 1) % points.shape[0]]
+        normal = np.cross(v1, v2)
+        normals.append(normal)
+    return np.array(normals)
+
+
+def project_points_onto_axis(points, axis):
+    projections = np.dot(points, axis)
+    return np.min(projections), np.max(projections)
+
+
+def roll_joint_collision(theta1, theta2, theta3):
+    end_effector_points = to_end_effector_points(theta1, theta2, theta3)
+
+    assert len(end_effector_points) == 8 and len(end_effector_points[0]) == 3
+    assert len(DRIVER_CAM_POINTS) == 8 and len(DRIVER_CAM_POINTS[0]) == 3
+
+    # Use the Separating Axis Theorem to check for collision
+    end_effector_normals = compute_face_normals(end_effector_points)
+    driver_cam_normals = compute_face_normals(DRIVER_CAM_POINTS)
+
+    collision = True
+    # Check for separating axes
+    for normal in np.concatenate((end_effector_normals, driver_cam_normals)):
+        min_ee, max_ee = project_points_onto_axis(end_effector_points, normal)
+        min_dc, max_dc = project_points_onto_axis(DRIVER_CAM_POINTS, normal)
+        if max_ee < min_dc or min_ee > max_dc:
+            # Separating axis found, rectangles don't intersect
+            collision = False
+            break
+
+    return collision
+
+
 def get_circular_index(theta):
-    return int(numpy.floor((theta[1] - theta[0]) / numpy.pi))
+    return int(np.floor((theta[1] - theta[0]) / np.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))
+    x = np.cos(theta1) * l1 + np.cos(theta2) * l2 + joint_center[0]
+    y = np.sin(theta1) * l1 + np.sin(theta2) * l2 + joint_center[1]
+    return np.array((x, y))
 
 
 # Subdivide in theta space.
@@ -70,29 +201,23 @@
     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)
+    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))
+    n_circular_index = int(np.floor((theta2 - theta1) / np.pi))
+    theta2 = theta2 + ((circular_index - n_circular_index)) * np.pi
+    return np.array((theta1, theta2))
+
+
+# to_theta, but distinguishes between
+def to_theta_with_circular_index_and_roll(x, y, roll, circular_index):
+    theta12 = to_theta_with_circular_index(x, y, circular_index)
+    theta3 = roll
+    return np.array((theta12[0], theta12[1], theta3))
 
 
 # alpha is in [0, 1] and is the weight to merge a and b.
@@ -108,83 +233,24 @@
 
 def normalize(v):
     """Normalize a vector while handling 0 length vectors."""
-    norm = numpy.linalg.norm(v)
+    norm = np.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))
+    dist = np.sqrt(dx**2 + dy**2)
+    n = int(np.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
-
-
 def spline_eval(start, control1, control2, end, alpha):
     a = alpha_blend(start, control1, alpha)
     b = alpha_blend(control1, control2, alpha)
@@ -193,19 +259,56 @@
                        alpha)
 
 
-def subdivide_spline(start, control1, control2, end):
+SPLINE_SUBDIVISIONS = 100
+
+
+def subdivide_multistep():
     # 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)
+    for i in range(0, SPLINE_SUBDIVISIONS + 1):
+        yield i / float(SPLINE_SUBDIVISIONS)
 
 
-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])
+def get_proximal_distal_derivs(t_prev, t, t_next):
+    d_prev = normalize(t - t_prev)
+    d_next = normalize(t_next - t)
+    accel = (d_next - d_prev) / np.linalg.norm(t - t_next)
+    return (ThetaPoint(t[0], d_next[0],
+                       accel[0]), ThetaPoint(t[1], d_next[1], accel[1]))
+
+
+def get_roll_joint_theta(theta_i, theta_f, t):
+    # Fit a theta(t) = (1 - cos(pi*t)) / 2,
+    # so that theta(0) = theta_i, and theta(1) = theta_f
+    offset = theta_i
+    scalar = (theta_f - theta_i) / 2.0
+    freq = np.pi
+    theta_curve = lambda t: scalar * (1 - np.cos(freq * t)) + offset
+
+    return theta_curve(t)
+
+
+def get_roll_joint_theta_multistep(alpha_rolls, alpha):
+    # Figure out which segment in the motion we're in
+    theta_i = None
+    theta_f = None
+    t = None
+
+    for i in range(len(alpha_rolls) - 1):
+        # Find the alpha segment we're in
+        if alpha_rolls[i][0] <= alpha <= alpha_rolls[i + 1][0]:
+            theta_i = alpha_rolls[i][1]
+            theta_f = alpha_rolls[i + 1][1]
+
+            total_dalpha = alpha_rolls[-1][0] - alpha_rolls[0][0]
+            assert total_dalpha == 1.0
+            dalpha = alpha_rolls[i + 1][0] - alpha_rolls[i][0]
+            t = (alpha - alpha_rolls[i][0]) * (total_dalpha / dalpha)
+            break
+    assert theta_i is not None
+    assert theta_f is not None
+    assert t is not None
+
+    return get_roll_joint_theta(theta_i, theta_f, t)
 
 
 # Draw a list of lines to a cairo context.
@@ -215,237 +318,105 @@
         cr.line_to(pt[0], pt[1])
 
 
-# Segment in angle space.
-class AngleSegment:
+class Path(abc.ABC):
 
-    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
+    def __init__(self, name):
         self.name = name
-        self.alpha_unitizer = alpha_unitizer
-        self.vmax = vmax
 
-    def __repr__(self):
-        return "AngleSegment(%s, %s)" % (repr(self.start), repr(self.end))
+    @abc.abstractmethod
+    def DoToThetaPoints(self):
+        pass
+
+    @abc.abstractmethod
+    def DoDrawTo(self):
+        pass
+
+    @abc.abstractmethod
+    def roll_joint_thetas(self):
+        pass
+
+    @abc.abstractmethod
+    def intersection(self, event):
+        pass
+
+    def roll_joint_collision(self, points, verbose=False):
+        for point in points:
+            if roll_joint_collision(*point):
+                if verbose:
+                    print("Roll joint collision for path %s in point %s" %
+                          (self.name, point))
+                return True
+        return False
 
     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)
+        if self.roll_joint_collision(self.DoToThetaPoints()):
+            cr.set_source_rgb(1.0, 0.0, 0.0)
+        self.DoDrawTo(cr, theta_version)
 
     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)]
+        points = self.DoToThetaPoints()
+        if self.roll_joint_collision(points, verbose=True):
+            sys.exit(1)
+        return points
 
 
-class XYSegment:
-    """Straight line in XY space."""
+class SplineSegmentBase(Path):
 
-    def __init__(self, start, end, name=None, alpha_unitizer=None, vmax=None):
-        """Creates an XY segment.
+    def __init__(self, name):
+        super().__init__(name)
 
-        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
+    @abc.abstractmethod
+    # Returns (start, control1, control2, end), each in the form
+    # (theta1, theta2, theta3)
+    def get_controls_theta(self):
+        pass
 
-    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 intersection(self, event):
+        start, control1, control2, end = self.get_controls_theta()
+        for alpha in subdivide_multistep():
+            x, y = get_xy(spline_eval(start, control1, control2, end, alpha))
+            spline_point = np.array([x, y])
+            hovered_point = np.array([event.x, event.y])
+            if np.linalg.norm(hovered_point - spline_point) < 0.03:
+                return alpha
+        return None
 
 
-class SplineSegment:
+class ThetaSplineSegment(SplineSegmentBase):
 
+    # start and end are [theta1, theta2, theta3].
+    # controls are just [theta1, theta2].
+    # control_alpha_rolls are a list of [alpha, roll]
     def __init__(self,
+                 name,
                  start,
                  control1,
                  control2,
                  end,
-                 name=None,
+                 control_alpha_rolls=[],
                  alpha_unitizer=None,
                  vmax=None):
-        self.start = start
+        super().__init__(name)
+        self.start = start[:2]
         self.control1 = control1
         self.control2 = control2
-        self.end = end
-        self.name = name
+        self.end = end[:2]
+        # There will always be roll at alpha = 0 and 1
+        self.alpha_rolls = [[0.0, start[2]]
+                            ] + control_alpha_rolls + [[1.0, end[2]]]
         self.alpha_unitizer = alpha_unitizer
         self.vmax = vmax
 
     def __repr__(self):
-        return "SplineSegment(%s, %s, %s, %s)" % (repr(
+        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:
-            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)
-        ]
-
-
-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):
+    def DoDrawTo(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)
+                            alpha) for alpha in subdivide_multistep()
             ])
         else:
             start = get_xy(self.start)
@@ -455,70 +426,34 @@
                 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)
+                for alpha in subdivide_multistep()
             ])
 
             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.arc(start[0], start[1], xy_end_circle_size, 0, 2.0 * np.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)
+            cr.arc(end[0], end[1], xy_end_circle_size, 0, 2.0 * np.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)
-        ]
+    def DoToThetaPoints(self):
+        points = []
+        for alpha in subdivide_multistep():
+            proximal, distal = spline_eval(self.start, self.control1,
+                                           self.control2, self.end, alpha)
+            roll_joint = get_roll_joint_theta_multistep(
+                self.alpha_rolls, alpha)
+            points.append((proximal, distal, roll_joint))
 
+        return points
 
-def expand_points(points, max_distance):
-    """Expands a list of points to be at most max_distance apart
+    def get_controls_theta(self):
+        return (self.start, self.control1, self.control2, self.end)
 
-    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
+    def roll_joint_thetas(self):
+        ts = []
+        thetas = []
+        for alpha in subdivide_multistep():
+            roll_joint = get_roll_joint_theta_multistep(
+                self.alpha_rolls, alpha)
+            thetas.append(roll_joint)
+            ts.append(alpha)
+        return ts, thetas