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/constants.cc b/y2023/constants.cc
index 123bf2b..1d6564e 100644
--- a/y2023/constants.cc
+++ b/y2023/constants.cc
@@ -40,24 +40,12 @@
arm_distal->zeroing.moving_buffer_size = 20;
arm_distal->zeroing.allowable_encoder_error = 0.9;
- ::frc971::control_loops::StaticZeroingSingleDOFProfiledSubsystemParams<
- ::frc971::zeroing::PotAndAbsoluteEncoderZeroingEstimator>
- *const roll_joint_params = &roll_joint->subsystem_params;
-
- roll_joint_params->zeroing_voltage = 3.0;
- roll_joint_params->operating_voltage = 12.0;
- roll_joint_params->zeroing_profile_params = {0.5, 3.0};
- roll_joint_params->default_profile_params = {6.0, 30.0};
- roll_joint_params->range = Values::kRollJointRange();
- roll_joint_params->make_integral_loop =
- control_loops::superstructure::roll::MakeIntegralRollLoop;
- roll_joint_params->zeroing_constants.average_filter_size =
- Values::kZeroingSampleSize;
- roll_joint_params->zeroing_constants.one_revolution_distance =
- M_PI * 2.0 * constants::Values::kRollJointEncoderRatio();
- roll_joint_params->zeroing_constants.zeroing_threshold = 0.0005;
- roll_joint_params->zeroing_constants.moving_buffer_size = 20;
- roll_joint_params->zeroing_constants.allowable_encoder_error = 0.9;
+ roll_joint->zeroing.average_filter_size = Values::kZeroingSampleSize;
+ roll_joint->zeroing.one_revolution_distance =
+ M_PI * 2.0 * constants::Values::kDistalEncoderRatio();
+ roll_joint->zeroing.zeroing_threshold = 0.0005;
+ roll_joint->zeroing.moving_buffer_size = 20;
+ roll_joint->zeroing.allowable_encoder_error = 0.9;
wrist->zeroing_voltage = 3.0;
wrist->operating_voltage = 12.0;
@@ -83,7 +71,7 @@
arm_distal->zeroing.measured_absolute_position = 0.0;
arm_distal->potentiometer_offset = 0.0;
- roll_joint_params->zeroing_constants.measured_absolute_position = 0.0;
+ roll_joint->zeroing.measured_absolute_position = 0.0;
roll_joint->potentiometer_offset = 0.0;
wrist->zeroing_constants.measured_absolute_position = 0.0;
@@ -97,7 +85,7 @@
arm_distal->zeroing.measured_absolute_position = 0.0;
arm_distal->potentiometer_offset = 0.0;
- roll_joint_params->zeroing_constants.measured_absolute_position = 0.0;
+ roll_joint->zeroing.measured_absolute_position = 0.0;
roll_joint->potentiometer_offset = 0.0;
wrist->zeroing_constants.measured_absolute_position = 0.0;
@@ -111,7 +99,7 @@
arm_distal->zeroing.measured_absolute_position = 0.0;
arm_distal->potentiometer_offset = 0.0;
- roll_joint_params->zeroing_constants.measured_absolute_position = 0.0;
+ roll_joint->zeroing.measured_absolute_position = 0.0;
roll_joint->potentiometer_offset = 0.0;
wrist->zeroing_constants.measured_absolute_position = 0.0;
@@ -125,7 +113,7 @@
arm_distal->zeroing.measured_absolute_position = 0.0;
arm_distal->potentiometer_offset = 0.0;
- roll_joint_params->zeroing_constants.measured_absolute_position = 0.0;
+ roll_joint->zeroing.measured_absolute_position = 0.0;
roll_joint->potentiometer_offset = 0.0;
wrist->zeroing_constants.measured_absolute_position = 0.0;
diff --git a/y2023/constants.h b/y2023/constants.h
index 0475242..66811a3 100644
--- a/y2023/constants.h
+++ b/y2023/constants.h
@@ -164,8 +164,7 @@
ArmJointConstants arm_proximal;
ArmJointConstants arm_distal;
-
- PotAndAbsEncoderConstants roll_joint;
+ ArmJointConstants roll_joint;
::frc971::control_loops::StaticZeroingSingleDOFProfiledSubsystemParams<
::frc971::zeroing::AbsoluteEncoderZeroingEstimator>
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
diff --git a/y2023/control_loops/superstructure/BUILD b/y2023/control_loops/superstructure/BUILD
index f5eff79..52254e4 100644
--- a/y2023/control_loops/superstructure/BUILD
+++ b/y2023/control_loops/superstructure/BUILD
@@ -117,6 +117,7 @@
"//frc971/control_loops:subsystem_simulator",
"//frc971/control_loops:team_number_test_environment",
"//frc971/control_loops/drivetrain:drivetrain_status_fbs",
+ "//y2023/control_loops/superstructure/roll:roll_plants",
],
)
diff --git a/y2023/control_loops/superstructure/arm/BUILD b/y2023/control_loops/superstructure/arm/BUILD
index 0ef2cf9..032c654 100644
--- a/y2023/control_loops/superstructure/arm/BUILD
+++ b/y2023/control_loops/superstructure/arm/BUILD
@@ -10,15 +10,15 @@
visibility = ["//visibility:public"],
deps = [
":generated_graph",
- "//frc971/control_loops/double_jointed_arm:demo_path",
"//frc971/control_loops/double_jointed_arm:ekf",
"//frc971/control_loops/double_jointed_arm:graph",
- "//frc971/control_loops/double_jointed_arm:trajectory",
"//frc971/zeroing",
"//y2023:constants",
"//y2023/control_loops/superstructure:superstructure_position_fbs",
"//y2023/control_loops/superstructure:superstructure_status_fbs",
"//y2023/control_loops/superstructure/arm:arm_constants",
+ "//y2023/control_loops/superstructure/arm:trajectory",
+ "//y2023/control_loops/superstructure/roll:roll_plants",
],
)
@@ -49,7 +49,8 @@
deps = [
":arm_constants",
"//frc971/control_loops/double_jointed_arm:graph",
- "//frc971/control_loops/double_jointed_arm:trajectory",
+ "//y2023/control_loops/superstructure/arm:trajectory",
+ "//y2023/control_loops/superstructure/roll:roll_plants",
],
)
diff --git a/y2023/control_loops/superstructure/arm/arm.cc b/y2023/control_loops/superstructure/arm/arm.cc
index 73781d2..5417dc8 100644
--- a/y2023/control_loops/superstructure/arm/arm.cc
+++ b/y2023/control_loops/superstructure/arm/arm.cc
@@ -1,5 +1,8 @@
#include "y2023/control_loops/superstructure/arm/arm.h"
+#include "y2023/control_loops/superstructure/roll/integral_hybrid_roll_plant.h"
+#include "y2023/control_loops/superstructure/roll/integral_roll_plant.h"
+
namespace y2023 {
namespace control_loops {
namespace superstructure {
@@ -18,20 +21,24 @@
state_(ArmState::UNINITIALIZED),
proximal_zeroing_estimator_(values_->arm_proximal.zeroing),
distal_zeroing_estimator_(values_->arm_distal.zeroing),
+ roll_joint_zeroing_estimator_(values_->roll_joint.zeroing),
proximal_offset_(0.0),
distal_offset_(0.0),
- max_intake_override_(1000.0),
- alpha_unitizer_((::Eigen::Matrix<double, 2, 2>() << 1.0 / kAlpha0Max(),
- 0.0, 0.0, 1.0 / kAlpha1Max())
+ roll_joint_offset_(0.0),
+ alpha_unitizer_((::Eigen::DiagonalMatrix<double, 3>().diagonal()
+ << (1.0 / kAlpha0Max()),
+ (1.0 / kAlpha1Max()), (1.0 / kAlpha2Max()))
.finished()),
dynamics_(kArmConstants),
- search_graph_(MakeSearchGraph(&dynamics_, &trajectories_, alpha_unitizer_,
- kVMax())),
close_enough_for_full_power_(false),
brownout_count_(0),
+ roll_joint_loop_(roll::MakeIntegralRollLoop()),
+ hybrid_roll_joint_loop_(roll::MakeIntegralHybridRollLoop()),
arm_ekf_(&dynamics_),
+ search_graph_(MakeSearchGraph(&dynamics_, &trajectories_, alpha_unitizer_,
+ kVMax(), &hybrid_roll_joint_loop_)),
// Go to the start of the first trajectory.
- follower_(&dynamics_, NeutralPosPoint()),
+ follower_(&dynamics_, &hybrid_roll_joint_loop_, NeutralPosPoint()),
points_(PointList()),
current_node_(0) {
int i = 0;
@@ -48,26 +55,32 @@
const ::aos::monotonic_clock::time_point /*monotonic_now*/,
const uint32_t *unsafe_goal, const superstructure::ArmPosition *position,
bool trajectory_override, double *proximal_output, double *distal_output,
- bool /*intake*/, bool /*spit*/, flatbuffers::FlatBufferBuilder *fbb) {
- ::Eigen::Matrix<double, 2, 1> Y;
+ double *roll_joint_output, bool /*intake*/, bool /*spit*/,
+ flatbuffers::FlatBufferBuilder *fbb) {
const bool outputs_disabled =
- ((proximal_output == nullptr) || (distal_output == nullptr));
+ ((proximal_output == nullptr) || (distal_output == nullptr) ||
+ (roll_joint_output == nullptr));
if (outputs_disabled) {
++brownout_count_;
} else {
brownout_count_ = 0;
}
- uint32_t filtered_goal = 0;
+ // TODO(milind): should we default to the closest position?
+ uint32_t filtered_goal = arm::NeutralPosIndex();
if (unsafe_goal != nullptr) {
filtered_goal = *unsafe_goal;
}
- Y << position->proximal()->encoder() + proximal_offset_,
+ ::Eigen::Matrix<double, 2, 1> Y_arm;
+ Y_arm << position->proximal()->encoder() + proximal_offset_,
position->distal()->encoder() + distal_offset_;
+ ::Eigen::Matrix<double, 1, 1> Y_roll_joint;
+ Y_roll_joint << position->roll_joint()->encoder() + roll_joint_offset_;
proximal_zeroing_estimator_.UpdateEstimate(*position->proximal());
distal_zeroing_estimator_.UpdateEstimate(*position->distal());
+ roll_joint_zeroing_estimator_.UpdateEstimate(*position->roll_joint());
if (proximal_output != nullptr) {
*proximal_output = 0.0;
@@ -75,15 +88,21 @@
if (distal_output != nullptr) {
*distal_output = 0.0;
}
+ if (roll_joint_output != nullptr) {
+ *roll_joint_output = 0.0;
+ }
- arm_ekf_.Correct(Y, kDt());
+ arm_ekf_.Correct(Y_arm, kDt());
+ roll_joint_loop_.Correct(Y_roll_joint);
if (::std::abs(arm_ekf_.X_hat(0) - follower_.theta(0)) <= 0.05 &&
- ::std::abs(arm_ekf_.X_hat(2) - follower_.theta(1)) <= 0.05) {
+ ::std::abs(arm_ekf_.X_hat(2) - follower_.theta(1)) <= 0.05 &&
+ ::std::abs(roll_joint_loop_.X_hat(0) - follower_.theta(2)) <= 0.05) {
close_enough_for_full_power_ = true;
}
if (::std::abs(arm_ekf_.X_hat(0) - follower_.theta(0)) >= 1.10 ||
- ::std::abs(arm_ekf_.X_hat(2) - follower_.theta(1)) >= 1.10) {
+ ::std::abs(arm_ekf_.X_hat(2) - follower_.theta(1)) >= 1.10 ||
+ ::std::abs(roll_joint_loop_.X_hat(0) - follower_.theta(2)) >= 0.50) {
close_enough_for_full_power_ = false;
}
@@ -94,25 +113,31 @@
state_ = ArmState::ZEROING;
proximal_zeroing_estimator_.Reset();
distal_zeroing_estimator_.Reset();
+ roll_joint_zeroing_estimator_.Reset();
break;
case ArmState::ZEROING:
// Zero by not moving.
- if (proximal_zeroing_estimator_.zeroed() &&
- distal_zeroing_estimator_.zeroed()) {
+ if (zeroed()) {
state_ = ArmState::DISABLED;
proximal_offset_ = proximal_zeroing_estimator_.offset();
distal_offset_ = distal_zeroing_estimator_.offset();
+ roll_joint_offset_ = roll_joint_zeroing_estimator_.offset();
- Y << position->proximal()->encoder() + proximal_offset_,
+ Y_arm << position->proximal()->encoder() + proximal_offset_,
position->distal()->encoder() + distal_offset_;
+ Y_roll_joint << position->roll_joint()->encoder() + roll_joint_offset_;
// TODO(austin): Offset ekf rather than reset it. Since we aren't
// moving at this point, it's pretty safe to do this.
- ::Eigen::Matrix<double, 4, 1> X;
- X << Y(0), 0.0, Y(1), 0.0;
- arm_ekf_.Reset(X);
+ ::Eigen::Matrix<double, 4, 1> X_arm;
+ X_arm << Y_arm(0), 0.0, Y_arm(1), 0.0;
+ arm_ekf_.Reset(X_arm);
+
+ ::Eigen::Matrix<double, 3, 1> X_roll_joint;
+ X_roll_joint << Y_roll_joint(0), 0.0, 0.0;
+ roll_joint_loop_.mutable_X_hat() = X_roll_joint;
} else {
break;
}
@@ -122,14 +147,14 @@
follower_.SwitchTrajectory(nullptr);
close_enough_for_full_power_ = false;
- const ::Eigen::Matrix<double, 2, 1> current_theta =
- (::Eigen::Matrix<double, 2, 1>() << arm_ekf_.X_hat(0),
- arm_ekf_.X_hat(2))
+ const ::Eigen::Matrix<double, 3, 1> current_theta =
+ (::Eigen::Matrix<double, 3, 1>() << arm_ekf_.X_hat(0),
+ arm_ekf_.X_hat(2), roll_joint_loop_.X_hat(0))
.finished();
uint32_t best_index = 0;
double best_distance = (points_[0] - current_theta).norm();
uint32_t current_index = 0;
- for (const ::Eigen::Matrix<double, 2, 1> &point : points_) {
+ for (const ::Eigen::Matrix<double, 3, 1> &point : points_) {
const double new_distance = (point - current_theta).norm();
if (new_distance < best_distance) {
best_distance = new_distance;
@@ -165,7 +190,8 @@
// ESTOP if we hit the hard limits.
// TODO(austin): Pick some sane limits.
if (proximal_zeroing_estimator_.error() ||
- distal_zeroing_estimator_.error()) {
+ distal_zeroing_estimator_.error() ||
+ roll_joint_zeroing_estimator_.error()) {
AOS_LOG(ERROR, "Zeroing error ESTOP\n");
state_ = ArmState::ESTOP;
} else if (outputs_disabled && brownout_count_ > kMaxBrownoutCount) {
@@ -227,38 +253,56 @@
close_enough_for_full_power_
? kOperatingVoltage()
: (state_ == ArmState::GOTO_PATH ? kGotoPathVMax() : kPathlessVMax());
- follower_.Update(arm_ekf_.X_hat(), disable, kDt(), vmax_,
- max_operating_voltage);
+ ::Eigen::Matrix<double, 9, 1> X_hat;
+ X_hat.block<6, 1>(0, 0) = arm_ekf_.X_hat();
+ X_hat.block<3, 1>(6, 0) = roll_joint_loop_.X_hat();
+
+ follower_.Update(X_hat, disable, kDt(), vmax_, max_operating_voltage);
AOS_LOG(INFO, "Max voltage: %f\n", max_operating_voltage);
+ arm_ekf_.Predict(follower_.U().head<2>(), kDt());
+ roll_joint_loop_.UpdateObserver(follower_.U().tail<1>(), kDtDuration());
+
flatbuffers::Offset<frc971::PotAndAbsoluteEncoderEstimatorState>
proximal_estimator_state_offset =
proximal_zeroing_estimator_.GetEstimatorState(fbb);
flatbuffers::Offset<frc971::PotAndAbsoluteEncoderEstimatorState>
distal_estimator_state_offset =
distal_zeroing_estimator_.GetEstimatorState(fbb);
+ flatbuffers::Offset<frc971::PotAndAbsoluteEncoderEstimatorState>
+ roll_joint_estimator_state_offset =
+ roll_joint_zeroing_estimator_.GetEstimatorState(fbb);
superstructure::ArmStatus::Builder status_builder(*fbb);
status_builder.add_proximal_estimator_state(proximal_estimator_state_offset);
status_builder.add_distal_estimator_state(distal_estimator_state_offset);
+ status_builder.add_roll_joint_estimator_state(
+ roll_joint_estimator_state_offset);
status_builder.add_goal_theta0(follower_.theta(0));
status_builder.add_goal_theta1(follower_.theta(1));
+ status_builder.add_goal_theta2(follower_.theta(2));
status_builder.add_goal_omega0(follower_.omega(0));
status_builder.add_goal_omega1(follower_.omega(1));
+ status_builder.add_goal_omega2(follower_.omega(2));
status_builder.add_theta0(arm_ekf_.X_hat(0));
status_builder.add_theta1(arm_ekf_.X_hat(2));
+ status_builder.add_theta2(roll_joint_loop_.X_hat(0));
status_builder.add_omega0(arm_ekf_.X_hat(1));
status_builder.add_omega1(arm_ekf_.X_hat(3));
+ status_builder.add_omega2(roll_joint_loop_.X_hat(1));
status_builder.add_voltage_error0(arm_ekf_.X_hat(4));
status_builder.add_voltage_error1(arm_ekf_.X_hat(5));
+ status_builder.add_voltage_error2(roll_joint_loop_.X_hat(2));
if (!disable) {
*proximal_output = ::std::max(
-kOperatingVoltage(), ::std::min(kOperatingVoltage(), follower_.U(0)));
*distal_output = ::std::max(
-kOperatingVoltage(), ::std::min(kOperatingVoltage(), follower_.U(1)));
+ *roll_joint_output = ::std::max(
+ -kOperatingVoltage(), ::std::min(kOperatingVoltage(), follower_.U(2)));
}
status_builder.add_path_distance_to_go(follower_.path_distance_to_go());
@@ -269,7 +313,6 @@
status_builder.add_state(state_);
status_builder.add_failed_solutions(follower_.failed_solutions());
- arm_ekf_.Predict(follower_.U(), kDt());
return status_builder.Finish();
}
diff --git a/y2023/control_loops/superstructure/arm/arm.h b/y2023/control_loops/superstructure/arm/arm.h
index 27588f1..9cda7fc 100644
--- a/y2023/control_loops/superstructure/arm/arm.h
+++ b/y2023/control_loops/superstructure/arm/arm.h
@@ -5,15 +5,14 @@
#include "frc971/control_loops/double_jointed_arm/dynamics.h"
#include "frc971/control_loops/double_jointed_arm/ekf.h"
#include "frc971/control_loops/double_jointed_arm/graph.h"
-#include "frc971/control_loops/double_jointed_arm/trajectory.h"
#include "frc971/zeroing/zeroing.h"
#include "y2023/constants.h"
#include "y2023/control_loops/superstructure/arm/generated_graph.h"
+#include "y2023/control_loops/superstructure/arm/trajectory.h"
#include "y2023/control_loops/superstructure/superstructure_position_generated.h"
#include "y2023/control_loops/superstructure/superstructure_status_generated.h"
using frc971::control_loops::arm::EKF;
-using frc971::control_loops::arm::TrajectoryFollower;
namespace y2023 {
namespace control_loops {
@@ -32,8 +31,13 @@
return kGrannyMode() ? 5.0 : 12.0;
}
static constexpr double kDt() { return 0.00505; }
+ static constexpr std::chrono::nanoseconds kDtDuration() {
+ return std::chrono::duration_cast<std::chrono::nanoseconds>(
+ std::chrono::duration<double>(kDt()));
+ }
static constexpr double kAlpha0Max() { return kGrannyMode() ? 5.0 : 15.0; }
static constexpr double kAlpha1Max() { return kGrannyMode() ? 5.0 : 15.0; }
+ static constexpr double kAlpha2Max() { return kGrannyMode() ? 5.0 : 15.0; }
static constexpr double kVMax() { return kGrannyMode() ? 5.0 : 11.5; }
static constexpr double kPathlessVMax() { return 5.0; }
@@ -43,7 +47,8 @@
const ::aos::monotonic_clock::time_point /*monotonic_now*/,
const uint32_t *unsafe_goal, const superstructure::ArmPosition *position,
bool trajectory_override, double *proximal_output, double *distal_output,
- bool /*intake*/, bool /*spit*/, flatbuffers::FlatBufferBuilder *fbb);
+ double *roll_joint_output, bool /*intake*/, bool /*spit*/,
+ flatbuffers::FlatBufferBuilder *fbb);
void Reset();
@@ -52,13 +57,10 @@
bool estopped() const { return state_ == ArmState::ESTOP; }
bool zeroed() const {
return (proximal_zeroing_estimator_.zeroed() &&
- distal_zeroing_estimator_.zeroed());
+ distal_zeroing_estimator_.zeroed() &&
+ roll_joint_zeroing_estimator_.zeroed());
}
- // Returns the maximum position for the intake. This is used to override the
- // intake position to release the box when the state machine is lifting.
- double max_intake_override() const { return max_intake_override_; }
-
uint32_t current_node() const { return current_node_; }
double path_distance_to_go() { return follower_.path_distance_to_go(); }
@@ -79,29 +81,36 @@
proximal_zeroing_estimator_;
::frc971::zeroing::PotAndAbsoluteEncoderZeroingEstimator
distal_zeroing_estimator_;
+ ::frc971::zeroing::PotAndAbsoluteEncoderZeroingEstimator
+ roll_joint_zeroing_estimator_;
double proximal_offset_;
double distal_offset_;
+ double roll_joint_offset_;
- double max_intake_override_;
-
- const ::Eigen::Matrix<double, 2, 2> alpha_unitizer_;
+ const ::Eigen::DiagonalMatrix<double, 3> alpha_unitizer_;
double vmax_ = kVMax();
frc971::control_loops::arm::Dynamics dynamics_;
::std::vector<TrajectoryAndParams> trajectories_;
- SearchGraph search_graph_;
bool close_enough_for_full_power_;
size_t brownout_count_;
+ StateFeedbackLoop<3, 1, 1, double, StateFeedbackPlant<3, 1, 1>,
+ StateFeedbackObserver<3, 1, 1>>
+ roll_joint_loop_;
+ const StateFeedbackLoop<3, 1, 1, double, StateFeedbackHybridPlant<3, 1, 1>,
+ HybridKalman<3, 1, 1>>
+ hybrid_roll_joint_loop_;
EKF arm_ekf_;
+ SearchGraph search_graph_;
TrajectoryFollower follower_;
- const ::std::vector<::Eigen::Matrix<double, 2, 1>> points_;
+ const ::std::vector<::Eigen::Matrix<double, 3, 1>> points_;
// Start at the 0th index.
uint32_t current_node_;
diff --git a/y2023/control_loops/superstructure/superstructure.cc b/y2023/control_loops/superstructure/superstructure.cc
index fa83d20..1ad2625 100644
--- a/y2023/control_loops/superstructure/superstructure.cc
+++ b/y2023/control_loops/superstructure/superstructure.cc
@@ -60,6 +60,7 @@
unsafe_goal != nullptr ? unsafe_goal->trajectory_override() : false,
output != nullptr ? &output_struct.proximal_voltage : nullptr,
output != nullptr ? &output_struct.distal_voltage : nullptr,
+ output != nullptr ? &output_struct.roll_joint_voltage : nullptr,
unsafe_goal != nullptr ? unsafe_goal->intake() : false,
unsafe_goal != nullptr ? unsafe_goal->spit() : false,
diff --git a/y2023/control_loops/superstructure/superstructure_lib_test.cc b/y2023/control_loops/superstructure/superstructure_lib_test.cc
index ae01f0b..f3f0471 100644
--- a/y2023/control_loops/superstructure/superstructure_lib_test.cc
+++ b/y2023/control_loops/superstructure/superstructure_lib_test.cc
@@ -9,6 +9,7 @@
#include "frc971/control_loops/team_number_test_environment.h"
#include "gtest/gtest.h"
#include "y2023/control_loops/drivetrain/drivetrain_dog_motor_plant.h"
+#include "y2023/control_loops/superstructure/roll/integral_roll_plant.h"
#include "y2023/control_loops/superstructure/superstructure.h"
DEFINE_string(output_folder, "",
@@ -50,6 +51,8 @@
&proximal_zeroing_constants,
const ::frc971::constants::PotAndAbsoluteEncoderZeroingConstants
&distal_zeroing_constants,
+ const ::frc971::constants::PotAndAbsoluteEncoderZeroingConstants
+ &roll_joint_zeroing_constants,
std::chrono::nanoseconds dt)
: proximal_zeroing_constants_(proximal_zeroing_constants),
proximal_pot_encoder_(M_PI * 2.0 *
@@ -57,12 +60,17 @@
distal_zeroing_constants_(distal_zeroing_constants),
distal_pot_encoder_(M_PI * 2.0 *
constants::Values::kDistalEncoderRatio()),
+ roll_joint_zeroing_constants_(roll_joint_zeroing_constants),
+ roll_joint_pot_encoder_(M_PI * 2.0 *
+ constants::Values::kDistalEncoderRatio()),
+ roll_joint_loop_(roll::MakeIntegralRollLoop()),
dynamics_(arm::kArmConstants),
dt_(dt) {
X_.setZero();
+ roll_joint_loop_.Reset();
}
- void InitializePosition(::Eigen::Matrix<double, 2, 1> position) {
+ void InitializePosition(::Eigen::Matrix<double, 3, 1> position) {
X_ << position(0), 0.0, position(1), 0.0;
proximal_pot_encoder_.Initialize(
@@ -71,6 +79,13 @@
distal_pot_encoder_.Initialize(
X_(2), kNoiseScalar, 0.0,
distal_zeroing_constants_.measured_absolute_position);
+
+ Eigen::Matrix<double, 3, 1> X_roll_joint;
+ X_roll_joint << position(2), 0.0, 0.0;
+ roll_joint_loop_.mutable_X_hat() = X_roll_joint;
+ roll_joint_pot_encoder_.Initialize(
+ X_roll_joint(0), kNoiseScalar, 0.0,
+ roll_joint_zeroing_constants_.measured_absolute_position);
}
flatbuffers::Offset<ArmPosition> GetSensorValues(
@@ -83,9 +98,14 @@
flatbuffers::Offset<frc971::PotAndAbsolutePosition> distal_offset =
distal_pot_encoder_.GetSensorValues(&distal_builder);
+ frc971::PotAndAbsolutePosition::Builder roll_joint_builder(*fbb);
+ flatbuffers::Offset<frc971::PotAndAbsolutePosition> roll_joint_offset =
+ roll_joint_pot_encoder_.GetSensorValues(&roll_joint_builder);
+
ArmPosition::Builder arm_position_builder(*fbb);
arm_position_builder.add_proximal(proximal_offset);
arm_position_builder.add_distal(distal_offset);
+ arm_position_builder.add_roll_joint(roll_joint_offset);
return arm_position_builder.Finish();
}
@@ -94,21 +114,26 @@
double proximal_velocity() const { return X_(1, 0); }
double distal_position() const { return X_(2, 0); }
double distal_velocity() const { return X_(3, 0); }
+ double roll_joint_position() const { return roll_joint_loop_.X_hat(0, 0); }
+ double roll_joint_velocity() const { return roll_joint_loop_.X_hat(1, 0); }
- void Simulate(::Eigen::Matrix<double, 2, 1> U) {
+ void Simulate(::Eigen::Matrix<double, 3, 1> U) {
constexpr double voltage_check =
superstructure::arm::Arm::kOperatingVoltage();
AOS_CHECK_LE(::std::abs(U(0)), voltage_check);
AOS_CHECK_LE(::std::abs(U(1)), voltage_check);
+ AOS_CHECK_LE(::std::abs(U(2)), voltage_check);
X_ = dynamics_.UnboundedDiscreteDynamics(
- X_, U,
+ X_, U.head<2>(),
std::chrono::duration_cast<std::chrono::duration<double>>(dt_).count());
+ roll_joint_loop_.UpdateObserver(U.tail<1>(), dt_);
// TODO(austin): Estop on grose out of bounds.
proximal_pot_encoder_.MoveTo(X_(0));
distal_pot_encoder_.MoveTo(X_(2));
+ roll_joint_pot_encoder_.MoveTo(roll_joint_loop_.X_hat(0));
}
private:
@@ -122,6 +147,13 @@
distal_zeroing_constants_;
PositionSensorSimulator distal_pot_encoder_;
+ const ::frc971::constants::PotAndAbsoluteEncoderZeroingConstants
+ roll_joint_zeroing_constants_;
+ PositionSensorSimulator roll_joint_pot_encoder_;
+ StateFeedbackLoop<3, 1, 1, double, StateFeedbackPlant<3, 1, 1>,
+ StateFeedbackObserver<3, 1, 1>>
+ roll_joint_loop_;
+
::frc971::control_loops::arm::Dynamics dynamics_;
std::chrono::nanoseconds dt_;
@@ -135,7 +167,8 @@
chrono::nanoseconds dt)
: event_loop_(event_loop),
dt_(dt),
- arm_(values->arm_proximal.zeroing, values->arm_distal.zeroing, dt_),
+ arm_(values->arm_proximal.zeroing, values->arm_distal.zeroing,
+ values->roll_joint.zeroing, dt_),
superstructure_position_sender_(
event_loop_->MakeSender<Position>("/superstructure")),
superstructure_status_fetcher_(
@@ -151,9 +184,10 @@
EXPECT_TRUE(superstructure_status_fetcher_.Fetch());
arm_.Simulate(
- (::Eigen::Matrix<double, 2, 1>()
+ (::Eigen::Matrix<double, 3, 1>()
<< superstructure_output_fetcher_->proximal_voltage(),
- superstructure_output_fetcher_->distal_voltage())
+ superstructure_output_fetcher_->distal_voltage(),
+ superstructure_output_fetcher_->roll_joint_voltage())
.finished());
}
first_ = false;
@@ -162,7 +196,7 @@
dt);
}
- void InitializeArmPosition(::Eigen::Matrix<double, 2, 1> position) {
+ void InitializeArmPosition(::Eigen::Matrix<double, 3, 1> position) {
arm_.InitializePosition(position);
}
@@ -224,8 +258,8 @@
test_event_loop_->MakeSender<DrivetrainStatus>("/drivetrain")),
superstructure_plant_event_loop_(MakeEventLoop("plant", roborio_)),
superstructure_plant_(superstructure_plant_event_loop_.get(), values_,
- dt()) {
- (void)values_;
+ dt()),
+ points_(arm::PointList()) {
set_team_id(frc971::control_loops::testing::kTeamNumber);
SetEnabled(true);
@@ -245,6 +279,48 @@
ASSERT_TRUE(superstructure_goal_fetcher_.get() != nullptr) << ": No goal";
ASSERT_TRUE(superstructure_status_fetcher_.get() != nullptr)
<< ": No status";
+
+ constexpr double kEpsTheta = 0.01;
+ constexpr double kEpsOmega = 0.01;
+
+ // Check that the status had the right goal
+ ASSERT_NEAR(points_[superstructure_goal_fetcher_->arm_goal_position()](0),
+ superstructure_status_fetcher_->arm()->goal_theta0(),
+ kEpsTheta);
+ ASSERT_NEAR(points_[superstructure_goal_fetcher_->arm_goal_position()](1),
+ superstructure_status_fetcher_->arm()->goal_theta1(),
+ kEpsTheta);
+ ASSERT_NEAR(points_[superstructure_goal_fetcher_->arm_goal_position()](2),
+ superstructure_status_fetcher_->arm()->goal_theta2(),
+ kEpsTheta);
+
+ // Check that the status met the goal
+ EXPECT_NEAR(superstructure_status_fetcher_->arm()->goal_theta0(),
+ superstructure_status_fetcher_->arm()->theta0(), kEpsTheta);
+ EXPECT_NEAR(superstructure_status_fetcher_->arm()->goal_theta1(),
+ superstructure_status_fetcher_->arm()->theta1(), kEpsTheta);
+ EXPECT_NEAR(superstructure_status_fetcher_->arm()->goal_theta2(),
+ superstructure_status_fetcher_->arm()->theta2(), kEpsTheta);
+ EXPECT_NEAR(superstructure_status_fetcher_->arm()->goal_omega0(),
+ superstructure_status_fetcher_->arm()->omega0(), kEpsOmega);
+ EXPECT_NEAR(superstructure_status_fetcher_->arm()->goal_omega1(),
+ superstructure_status_fetcher_->arm()->omega1(), kEpsOmega);
+ EXPECT_NEAR(superstructure_status_fetcher_->arm()->goal_omega2(),
+ superstructure_status_fetcher_->arm()->omega2(), kEpsOmega);
+
+ // Check that our simulator matches the status
+ EXPECT_NEAR(superstructure_plant_.arm().proximal_position(),
+ superstructure_status_fetcher_->arm()->theta0(), kEpsTheta);
+ EXPECT_NEAR(superstructure_plant_.arm().distal_position(),
+ superstructure_status_fetcher_->arm()->theta1(), kEpsTheta);
+ EXPECT_NEAR(superstructure_plant_.arm().roll_joint_position(),
+ superstructure_status_fetcher_->arm()->theta2(), kEpsTheta);
+ EXPECT_NEAR(superstructure_plant_.arm().proximal_velocity(),
+ superstructure_status_fetcher_->arm()->omega0(), kEpsOmega);
+ EXPECT_NEAR(superstructure_plant_.arm().distal_velocity(),
+ superstructure_status_fetcher_->arm()->omega1(), kEpsOmega);
+ EXPECT_NEAR(superstructure_plant_.arm().roll_joint_velocity(),
+ superstructure_status_fetcher_->arm()->omega2(), kEpsOmega);
}
void CheckIfZeroed() {
@@ -308,6 +384,8 @@
std::unique_ptr<aos::EventLoop> logger_event_loop_;
std::unique_ptr<aos::logger::Logger> logger_;
+
+ const ::std::vector<::Eigen::Matrix<double, 3, 1>> points_;
}; // namespace testing
// Tests that the superstructure does nothing when the goal is to remain
@@ -378,7 +456,6 @@
ASSERT_EQ(builder.Send(goal_builder.Finish()), aos::RawSender::Error::kOk);
}
- // Intake needs over 9 seconds to reach the goal
// TODO(Milo): Make this a sane time
RunFor(chrono::seconds(20));
VerifyNearGoal();
@@ -406,7 +483,7 @@
auto builder = superstructure_goal_sender_.MakeBuilder();
Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
- goal_builder.add_arm_goal_position(arm::NeutralToConePos1Index());
+ goal_builder.add_arm_goal_position(arm::PickupPosIndex());
ASSERT_EQ(builder.Send(goal_builder.Finish()), aos::RawSender::Error::kOk);
}
@@ -420,31 +497,6 @@
{
auto builder = superstructure_goal_sender_.MakeBuilder();
Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
- goal_builder.add_arm_goal_position(arm::NeutralToConePos1Index());
- ASSERT_EQ(builder.Send(goal_builder.Finish()), aos::RawSender::Error::kOk);
- }
-
- RunFor(chrono::seconds(10));
-
- VerifyNearGoal();
-
- {
- auto builder = superstructure_goal_sender_.MakeBuilder();
- Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
- goal_builder.add_arm_goal_position(arm::NeutralToConePos2Index());
- ASSERT_EQ(builder.Send(goal_builder.Finish()), aos::RawSender::Error::kOk);
- }
-
- RunFor(chrono::seconds(10));
- VerifyNearGoal();
-}
-
-// Tests that we can can execute a move which moves through multiple nodes.
-TEST_F(SuperstructureTest, ArmMultistepMove) {
- SetEnabled(true);
- {
- auto builder = superstructure_goal_sender_.MakeBuilder();
- Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
goal_builder.add_arm_goal_position(arm::NeutralPosIndex());
ASSERT_EQ(builder.Send(goal_builder.Finish()), aos::RawSender::Error::kOk);
}
@@ -456,7 +508,34 @@
{
auto builder = superstructure_goal_sender_.MakeBuilder();
Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
- goal_builder.add_arm_goal_position(arm::ConePosIndex());
+ goal_builder.add_arm_goal_position(arm::PickupPosIndex());
+ ASSERT_EQ(builder.Send(goal_builder.Finish()), aos::RawSender::Error::kOk);
+ }
+
+ RunFor(chrono::seconds(10));
+ VerifyNearGoal();
+}
+
+// Tests that we can can execute a move which moves through multiple nodes.
+TEST_F(SuperstructureTest, ArmMultistepMove) {
+ SetEnabled(true);
+ superstructure_plant_.InitializeArmPosition(arm::NeutralPosPoint());
+
+ {
+ auto builder = superstructure_goal_sender_.MakeBuilder();
+ Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+ goal_builder.add_arm_goal_position(arm::PickupPosIndex());
+ ASSERT_EQ(builder.Send(goal_builder.Finish()), aos::RawSender::Error::kOk);
+ }
+
+ RunFor(chrono::seconds(10));
+
+ VerifyNearGoal();
+
+ {
+ auto builder = superstructure_goal_sender_.MakeBuilder();
+ Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+ goal_builder.add_arm_goal_position(arm::ScorePosIndex());
ASSERT_EQ(builder.Send(goal_builder.Finish()), aos::RawSender::Error::kOk);
}
diff --git a/y2023/control_loops/superstructure/superstructure_position.fbs b/y2023/control_loops/superstructure/superstructure_position.fbs
index 8549943..bd3d607 100644
--- a/y2023/control_loops/superstructure/superstructure_position.fbs
+++ b/y2023/control_loops/superstructure/superstructure_position.fbs
@@ -12,18 +12,18 @@
// (connected to proximal) arm in radians.
// Zero is straight up, positive is a forwards rotation
distal:frc971.PotAndAbsolutePosition (id: 1);
+
+ // Zero for roll joint is up vertically
+ // Positive position would be rotated counterclockwise relative to the robot
+ roll_joint:frc971.PotAndAbsolutePosition (id: 2);
}
table Position {
arm:ArmPosition (id: 0);
- // Zero for roll joint is up vertically
- // Positive position would be rotated counterclockwise relative to the robot
- roll_joint:frc971.PotAndAbsolutePosition (id: 1);
-
// Zero for wrist is facing staright outward.
// Positive position would be upwards
- wrist:frc971.AbsolutePosition (id: 2);
+ wrist:frc971.AbsolutePosition (id: 1);
}
root_type Position;
diff --git a/y2023/control_loops/superstructure/superstructure_status.fbs b/y2023/control_loops/superstructure/superstructure_status.fbs
index 9b21cc3..db86687 100644
--- a/y2023/control_loops/superstructure/superstructure_status.fbs
+++ b/y2023/control_loops/superstructure/superstructure_status.fbs
@@ -16,6 +16,7 @@
// State of the estimators.
proximal_estimator_state:frc971.PotAndAbsoluteEncoderEstimatorState (id: 0);
distal_estimator_state:frc971.PotAndAbsoluteEncoderEstimatorState (id: 1);
+ roll_joint_estimator_state:frc971.PotAndAbsoluteEncoderEstimatorState (id: 18);
// The node we are currently going to.
current_node:uint32 (id: 2);
@@ -24,19 +25,24 @@
// Goal position and velocity (radians)
goal_theta0:float (id: 4);
goal_theta1:float (id: 5);
+ goal_theta2:float (id: 19);
goal_omega0:float (id: 6);
goal_omega1:float (id: 7);
+ goal_omega2:float (id: 20);
// Current position and velocity (radians)
theta0:float (id: 8);
theta1:float (id: 9);
+ theta2:float (id: 21);
omega0:float (id: 10);
omega1:float (id: 11);
+ omega2:float (id: 22);
// Estimated voltage error for the two joints.
voltage_error0:float (id: 12);
voltage_error1:float (id: 13);
+ voltage_error2:float (id: 23);
// True if we are zeroed.
zeroed:bool (id: 14);
@@ -60,9 +66,7 @@
arm:ArmStatus (id: 2);
- roll_joint:frc971.PotAndAbsoluteEncoderEstimatorState (id: 3);
-
- wrist:frc971.control_loops.AbsoluteEncoderProfiledJointStatus (id: 4);
+ wrist:frc971.control_loops.AbsoluteEncoderProfiledJointStatus (id: 3);
}
root_type Status;
diff --git a/y2023/joystick_reader.cc b/y2023/joystick_reader.cc
index 3d019f1..3caaa41 100644
--- a/y2023/joystick_reader.cc
+++ b/y2023/joystick_reader.cc
@@ -72,12 +72,12 @@
// TODO(milind): add more actions and paths
if (data.IsPressed(kIntake)) {
intake = true;
- arm_goal_position_ = arm::ConePosIndex();
+ arm_goal_position_ = arm::PickupPosIndex();
} else if (data.IsPressed(kSpit)) {
spit = true;
- arm_goal_position_ = arm::ConePosIndex();
+ arm_goal_position_ = arm::PickupPosIndex();
} else if (data.IsPressed(kScore)) {
- arm_goal_position_ = arm::ConePerchPosIndex();
+ arm_goal_position_ = arm::ScorePosIndex();
}
{
diff --git a/y2023/wpilib_interface.cc b/y2023/wpilib_interface.cc
index da2bebb..fa1f09e 100644
--- a/y2023/wpilib_interface.cc
+++ b/y2023/wpilib_interface.cc
@@ -184,11 +184,11 @@
frc971::PotAndAbsolutePosition::Pack(*builder.fbb(), &proximal);
flatbuffers::Offset<frc971::PotAndAbsolutePosition> distal_offset =
frc971::PotAndAbsolutePosition::Pack(*builder.fbb(), &distal);
- flatbuffers::Offset<superstructure::ArmPosition> arm_offset =
- superstructure::CreateArmPosition(*builder.fbb(), proximal_offset,
- distal_offset);
flatbuffers::Offset<frc971::PotAndAbsolutePosition> roll_joint_offset =
frc971::PotAndAbsolutePosition::Pack(*builder.fbb(), &roll_joint);
+ flatbuffers::Offset<superstructure::ArmPosition> arm_offset =
+ superstructure::CreateArmPosition(*builder.fbb(), proximal_offset,
+ distal_offset, roll_joint_offset);
flatbuffers::Offset<frc971::AbsolutePosition> wrist_offset =
frc971::AbsolutePosition::Pack(*builder.fbb(), &wrist);
@@ -196,7 +196,6 @@
builder.MakeBuilder<superstructure::Position>();
position_builder.add_arm(arm_offset);
- position_builder.add_roll_joint(roll_joint_offset);
position_builder.add_wrist(wrist_offset);
builder.CheckOk(builder.Send(position_builder.Finish()));
}