Add roll joint to superstructure and arm UI
Arm UI changes:
- Update robot dimensions
- Support visualizing roll joint
- Add roll joint collision detection
Superstructure changes:
- Adding roll joint feedback loop and zeroing estimator
Signed-off-by: milind-u <milind.upadhyay@gmail.com>
Change-Id: I422e343890248940bba98ba3cabac94e68723a3e
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