Make arm spline definition a ton easier
Remove all the redundant intermediate variables.
Change-Id: I1e1d93b8a4363f3498775a0eead3d14ae190e77a
Signed-off-by: Austin Schuh <austin.linux@gmail.com>
diff --git a/y2023/control_loops/python/graph_codegen.py b/y2023/control_loops/python/graph_codegen.py
index 054a32d..632f112 100644
--- a/y2023/control_loops/python/graph_codegen.py
+++ b/y2023/control_loops/python/graph_codegen.py
@@ -46,13 +46,14 @@
start_index = None
end_index = None
- for point, name in graph_paths.points:
+ for key in sorted(graph_paths.points.keys()):
+ point = graph_paths.points[key]
if (point[:2] == segment.start
).all() and point[2] == segment.alpha_rolls[0][1]:
- start_index = name
+ start_index = key
if (point[:2] == segment.end
).all() and point[2] == segment.alpha_rolls[-1][1]:
- end_index = name
+ end_index = key
if reverse:
start_index, end_index = end_index, start_index
@@ -146,15 +147,15 @@
h_file.append("")
# Now dump out the vertices and associated constexpr vertex name functions.
- for index, point in enumerate(graph_paths.points):
+ for index, key in enumerate(sorted(graph_paths.points.keys())):
+ point = graph_paths.points[key]
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, 3, 1> %sPoint() {" %
- point[1])
+ (index_function_name(key), index))
+ h_file.append("inline ::Eigen::Matrix<double, 3, 1> %sPoint() {" % key)
h_file.append(
" return (::Eigen::Matrix<double, 3, 1>() << %f, %f, %f).finished();"
- % (point[0][0], point[0][1], point[0][2]))
+ % (point[0], point[1], point[2]))
h_file.append("}")
front_points = [
@@ -214,10 +215,11 @@
cc_file.append(
"::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:
+ for key in sorted(graph_paths.points.keys()):
+ point = graph_paths.points[key]
cc_file.append(
" points.push_back((::Eigen::Matrix<double, 3, 1>() << %.12s, %.12s, %.12s).finished());"
- % (point[0][0], point[0][1], point[0][2]))
+ % (point[0], point[1], point[2]))
cc_file.append(" return points;")
cc_file.append("}")
diff --git a/y2023/control_loops/python/graph_edit.py b/y2023/control_loops/python/graph_edit.py
index d841509..a976807 100644
--- a/y2023/control_loops/python/graph_edit.py
+++ b/y2023/control_loops/python/graph_edit.py
@@ -217,7 +217,7 @@
self.theta_version = False
self.reinit_extents()
- self.last_pos = to_xy(*graph_paths.neutral[:2])
+ self.last_pos = to_xy(*graph_paths.points['Neutral'][:2])
self.circular_index_select = 1
# Extra stuff for drawing lines.
@@ -494,11 +494,13 @@
else:
self.index = len(self.segments) - 1
print("Switched to segment:", self.segments[self.index].name)
+ self.segments[self.index].Print(graph_paths.points)
elif keyval == Gdk.KEY_n:
self.index += 1
self.index = self.index % len(self.segments)
print("Switched to segment:", self.segments[self.index].name)
+ self.segments[self.index].Print(graph_paths.points)
elif keyval == Gdk.KEY_t:
# Toggle between theta and xy renderings
@@ -546,18 +548,15 @@
else:
self.segments[self.index].control2 = self.now_segment_pt
- print('Clicked at theta: %s' % (repr(self.now_segment_pt, )))
+ print('Clicked at theta: np.array([%s, %s])' %
+ (self.now_segment_pt[0], self.now_segment_pt[1]))
if not self.theta_version:
- print('Clicked at xy, circular index: (%f, %f, %f)' %
- (self.last_pos[0], self.last_pos[1],
+ print(
+ 'Clicked at to_theta_with_circular_index(%.3f, %.3f, circular_index=%d)'
+ % (self.last_pos[0], self.last_pos[1],
self.circular_index_select))
- print('c1: np.array([%f, %f])' %
- (self.segments[self.index].control1[0],
- self.segments[self.index].control1[1]))
- print('c2: np.array([%f, %f])' %
- (self.segments[self.index].control2[0],
- self.segments[self.index].control2[1]))
+ self.segments[self.index].Print(graph_paths.points)
self.redraw()
@@ -565,4 +564,5 @@
arm_ui = ArmUi()
arm_ui.segments = graph_paths.segments
print('Starting with segment: ', arm_ui.segments[arm_ui.index].name)
+arm_ui.segments[arm_ui.index].Print(graph_paths.points)
basic_window.RunApp()
diff --git a/y2023/control_loops/python/graph_paths.py b/y2023/control_loops/python/graph_paths.py
index 66ca158..9cfdc13 100644
--- a/y2023/control_loops/python/graph_paths.py
+++ b/y2023/control_loops/python/graph_paths.py
@@ -2,186 +2,130 @@
from y2023.control_loops.python.graph_tools import *
-neutral = np.array((np.pi, 0.0, 0.0))
+named_segments = []
+points = {}
-# NeutralToGroundPickupBackConeUp
-neutral_to_cone_up_1 = np.array([3.170156, -0.561227])
-neutral_to_cone_up_2 = np.array([2.972776, -1.026820])
-ground_pickup_back_cone_up = to_theta_with_circular_index_and_roll(
+points['Neutral'] = np.array((np.pi, 0.0, 0.0))
+
+points['GroundPickupBackConeUp'] = to_theta_with_circular_index_and_roll(
-1.07774334, 0.36308701, np.pi / 2.0, circular_index=1)
-# NeutralToGroundPickupBackConeDown
-neutral_to_ground_pickup_back_cone_down_1 = np.array([3.170156, -0.561227])
-neutral_to_ground_pickup_back_cone_down_2 = np.array([2.972776, -1.026820])
-ground_pickup_back_cone_down = to_theta_with_circular_index_and_roll(
+named_segments.append(
+ ThetaSplineSegment(
+ name="NeutralToGroundPickupBackConeUp",
+ start=points['Neutral'],
+ control1=np.array([3.170156, -0.561227]),
+ control2=np.array([2.972776, -1.026820]),
+ end=points['GroundPickupBackConeUp'],
+ control_alpha_rolls=[(0.30, 0.0), (.95, np.pi / 2.0)],
+ ))
+
+points['GroundPickupBackConeDown'] = to_theta_with_circular_index_and_roll(
-1.11487594, 0.23140145, np.pi / 2.0, circular_index=1)
-# NeutralToGroundPickupBackCube
-neutral_to_ground_pickup_back_cube_1 = np.array([3.153228, -0.497009])
-neutral_to_ground_pickup_back_cube_2 = np.array([2.972776, -1.026820])
-neutral_to_hp_pickup_back_cube_alpha_rolls = [
- (0.7, 0.0),
- (.9, -np.pi / 2.0),
-]
-ground_pickup_back_cube = to_theta_with_circular_index_and_roll(
+named_segments.append(
+ ThetaSplineSegment(
+ name="NeutralToGroundPickupBackConeDown",
+ start=points['Neutral'],
+ control1=np.array([3.170156, -0.561227]),
+ control2=np.array([2.972776, -1.026820]),
+ end=points['GroundPickupBackConeDown'],
+ control_alpha_rolls=[(0.30, 0.0), (.95, np.pi / 2.0)],
+ ))
+
+points['GroundPickupBackCube'] = to_theta_with_circular_index_and_roll(
-1.102, 0.224, -np.pi / 2.0, circular_index=1)
-# NeutralToBackMidConeUpScore
-neutral_to_score_back_mid_cone_up_1 = np.array([0.994244, -1.417442])
-neutral_to_score_back_mid_cone_up_2 = np.array([1.711325, -0.679748])
-score_back_mid_cone_up_pos = to_theta_with_circular_index_and_roll(
+named_segments.append(
+ ThetaSplineSegment(
+ name="NeutralToGroundPickupBackCube",
+ start=points['Neutral'],
+ control1=np.array([3.153228, -0.497009]),
+ control2=np.array([2.972776, -1.026820]),
+ end=points['GroundPickupBackCube'],
+ control_alpha_rolls=[(0.7, 0.0), (.9, -np.pi / 2.0)],
+ ))
+
+points['ScoreBackMidConeUpPos'] = to_theta_with_circular_index_and_roll(
-1.41871454, 1.07476162, np.pi / 2.0, circular_index=0)
-# NeutralToMidConeDownScore
-neutral_to_score_mid_cone_down_1 = np.array([3.394572, -0.239378])
-neutral_to_score_mid_cone_down_2 = np.array([3.654854, -0.626835])
-score_mid_cone_down_pos = to_theta_with_circular_index_and_roll(
+named_segments.append(
+ ThetaSplineSegment(
+ name="NeutralToBackMidConeUpScore",
+ start=points['Neutral'],
+ control1=np.array([0.994244, -1.417442]),
+ control2=np.array([1.711325, -0.679748]),
+ end=points['ScoreBackMidConeUpPos'],
+ control_alpha_rolls=[(0.40, 0.0), (.95, np.pi / 2.0)],
+ ))
+
+points['ScoreBackMidConeDownPos'] = to_theta_with_circular_index_and_roll(
-1.37792406, 0.81332449, np.pi / 2.0, circular_index=1)
-# NeutralToMidConeDownScore
-neutral_to_hp_pickup_back_cone_up_1 = np.array([2.0, -0.239378])
-neutral_to_hp_pickup_back_cone_up_2 = np.array([1.6, -0.626835])
-neutral_to_hp_pickup_back_cone_up_alpha_rolls = [
- (0.7, 0.0),
- (.9, np.pi / 2.0),
-]
-hp_pickup_back_cone_up = to_theta_with_circular_index_and_roll(
+named_segments.append(
+ ThetaSplineSegment(
+ name="NeutralToMidConeDownScore",
+ start=points['Neutral'],
+ control1=np.array([3.394572, -0.239378]),
+ control2=np.array([3.654854, -0.626835]),
+ end=points['ScoreBackMidConeDownPos'],
+ control_alpha_rolls=[(0.40, 0.0), (.95, np.pi / 2.0)],
+ ))
+
+points['HPPickupBackConeUp'] = to_theta_with_circular_index_and_roll(
-1.1050539, 1.31390128, np.pi / 2.0, circular_index=0)
-# NeutralToFrontHighConeUpScore
-neutral_to_score_front_high_cone_up_1 = np.array([2.594244, 0.417442])
-neutral_to_score_front_high_cone_up_2 = np.array([1.51325, 0.679748])
-score_front_high_cone_up_pos = to_theta_with_circular_index_and_roll(
+named_segments.append(
+ ThetaSplineSegment(
+ name="NeutralToHPPickupBackConeUp",
+ start=points['Neutral'],
+ control1=np.array([2.0, -0.239378]),
+ control2=np.array([1.6, -0.626835]),
+ end=points['HPPickupBackConeUp'],
+ control_alpha_rolls=[(0.7, 0.0), (.9, np.pi / 2.0)],
+ ))
+
+points['ScoreFrontHighConeUpPos'] = to_theta_with_circular_index_and_roll(
0.98810344, 1.37536719, -np.pi / 2.0, circular_index=0)
-# NeutralToFrontMidConeUpScore
-neutral_to_score_front_mid_cone_up_1 = np.array([3.0, 0.317442])
-neutral_to_score_front_mid_cone_up_2 = np.array([2.9, 0.479748])
-score_front_mid_cone_up_pos = to_theta_with_circular_index_and_roll(
+named_segments.append(
+ ThetaSplineSegment(
+ name="NeutralToFrontHighConeUpScore",
+ start=points['Neutral'],
+ control1=np.array([2.594244, 0.417442]),
+ control2=np.array([1.51325, 0.679748]),
+ end=points['ScoreFrontHighConeUpPos'],
+ control_alpha_rolls=[(0.40, 0.0), (.95, -np.pi / 2.0)],
+ ))
+
+points['ScoreFrontMidConeUpPos'] = to_theta_with_circular_index_and_roll(
0.43740453, 1.06330555, -np.pi / 2.0, circular_index=0)
-neutral_to_cone_down_1 = np.array([2.396694, 0.508020])
-neutral_to_cone_down_2 = np.array([2.874513, 0.933160])
-cone_down_pos = to_theta_with_circular_index_and_roll(0.7,
- 0.11,
- np.pi / 2.0,
- circular_index=0)
+named_segments.append(
+ ThetaSplineSegment(
+ name="NeutralToFrontMidConeUpScore",
+ start=points['Neutral'],
+ control1=np.array([3.0, 0.317442]),
+ control2=np.array([2.9, 0.479748]),
+ end=points['ScoreFrontMidConeUpPos'],
+ control_alpha_rolls=[(0.40, 0.0), (.95, -np.pi / 2.0)],
+ ))
-neutral_to_cube_1 = np.array([2.396694, 0.508020])
-neutral_to_cube_2 = np.array([2.874513, 0.933160])
+points['ConeDownPos'] = to_theta_with_circular_index_and_roll(0.7,
+ 0.11,
+ np.pi / 2.0,
+ circular_index=0)
+named_segments.append(
+ ThetaSplineSegment(
+ name="NeutralToConeDown",
+ start=points['Neutral'],
+ control1=np.array([2.396694, 0.508020]),
+ control2=np.array([2.874513, 0.933160]),
+ end=points['ConeDownPos'],
+ control_alpha_rolls=[(0.30, 0.0), (.95, np.pi / 2.0)],
+ ))
-cube_pos = to_theta_with_circular_index_and_roll(0.7,
- 0.24,
- np.pi / 2.0,
- circular_index=0)
-
-neutral_to_pickup_control_alpha_rolls = [
- (0.30, 0.0),
- (.95, np.pi / 2.0),
-]
-
-neutral_to_score_1 = np.array([0.994244, -1.417442])
-neutral_to_score_2 = np.array([1.711325, -0.679748])
-
-score_low_pos = to_theta_with_circular_index_and_roll(-(0.41 / 2 + 0.49),
- 0 + 0.05,
- np.pi / 2.0,
- circular_index=1)
-
-neutral_to_score_low_2 = np.array([3.37926599, -0.73664663])
-
-score_mid_cube_pos = to_theta_with_circular_index_and_roll(-(0.58 + 0.49),
- 0.6 + 0.05,
- np.pi / 2.0,
- circular_index=0)
-
-score_high_cone_pos = to_theta_with_circular_index_and_roll(-1.01,
- 1.17 + 0.05,
- np.pi / 2.0,
- circular_index=0)
-
-score_high_cube_pos = to_theta_with_circular_index_and_roll(-1.01,
- 0.90 + 0.05,
- np.pi / 2.0,
- circular_index=0)
-
-neutral_to_back_score_control_alpha_rolls = [(0.40, 0.0), (.95, np.pi / 2.0)]
-neutral_to_front_score_control_alpha_rolls = [(0.40, 0.0), (.95, -np.pi / 2.0)]
-
-points = [(neutral, "NeutralPos"),
- (ground_pickup_back_cone_up, "GroundPickupBackConeUp"),
- (ground_pickup_back_cone_down, "GroundPickupBackConeDown"),
- (ground_pickup_back_cube, "GroundPickupBackCube"),
- (hp_pickup_back_cone_up, "HPPickupBackConeUp"),
- (cone_down_pos, "ConeDownPos"), (score_low_pos, "ScoreLowPos"),
- (score_back_mid_cone_up_pos, "ScoreBackMidConeUpPos"),
- (score_front_high_cone_up_pos, "ScoreFrontHighConeUpPos"),
- (score_front_mid_cone_up_pos, "ScoreFrontMidConeUpPos"),
- (score_mid_cone_down_pos, "ScoreBackMidConeDownPos"),
- (score_mid_cube_pos, "ScoreMidCubePos"),
- (score_high_cone_pos, "ScoreHighConePos"),
- (score_high_cube_pos, "ScoreHighCubePos"), (cube_pos, "CubePos")]
front_points = []
back_points = []
unnamed_segments = []
-named_segments = [
- ThetaSplineSegment("NeutralToGroundPickupBackConeUp", neutral,
- neutral_to_cone_up_1, neutral_to_cone_up_2,
- ground_pickup_back_cone_up,
- neutral_to_pickup_control_alpha_rolls),
- ThetaSplineSegment("NeutralToGroundPickupBackConeDown", neutral,
- neutral_to_ground_pickup_back_cone_down_1,
- neutral_to_ground_pickup_back_cone_down_2,
- ground_pickup_back_cone_down,
- neutral_to_pickup_control_alpha_rolls),
- ThetaSplineSegment("NeutralToGroundPickupBackCube", neutral,
- neutral_to_ground_pickup_back_cube_1,
- neutral_to_ground_pickup_back_cube_2,
- ground_pickup_back_cube,
- neutral_to_hp_pickup_back_cube_alpha_rolls),
- ThetaSplineSegment("NeutralToHPPickupBackConeUp", neutral,
- neutral_to_hp_pickup_back_cone_up_1,
- neutral_to_hp_pickup_back_cone_up_2,
- hp_pickup_back_cone_up,
- neutral_to_hp_pickup_back_cone_up_alpha_rolls),
- ThetaSplineSegment("NeutralToFrontHighConeUpScore", neutral,
- neutral_to_score_front_high_cone_up_1,
- neutral_to_score_front_high_cone_up_2,
- score_front_high_cone_up_pos,
- neutral_to_front_score_control_alpha_rolls),
- ThetaSplineSegment("NeutralToFrontMidConeUpScore", neutral,
- neutral_to_score_front_mid_cone_up_1,
- neutral_to_score_front_mid_cone_up_2,
- score_front_mid_cone_up_pos,
- neutral_to_front_score_control_alpha_rolls),
- ThetaSplineSegment("NeutralToConeDown", neutral, neutral_to_cone_down_1,
- neutral_to_cone_down_2, cone_down_pos,
- neutral_to_pickup_control_alpha_rolls),
- ThetaSplineSegment("NeutralToCube", neutral, neutral_to_cube_1,
- neutral_to_cube_2, cube_pos,
- neutral_to_pickup_control_alpha_rolls),
- ThetaSplineSegment("NeutralToLowScore", neutral, neutral_to_score_1,
- neutral_to_score_low_2, score_low_pos,
- neutral_to_back_score_control_alpha_rolls),
- ThetaSplineSegment("NeutralToBackMidConeUpScore", neutral,
- neutral_to_score_back_mid_cone_up_1,
- neutral_to_score_back_mid_cone_up_2,
- score_back_mid_cone_up_pos,
- neutral_to_back_score_control_alpha_rolls),
- ThetaSplineSegment("NeutralToMidConeDownScore", neutral,
- neutral_to_score_mid_cone_down_1,
- neutral_to_score_mid_cone_down_2,
- score_mid_cone_down_pos,
- neutral_to_back_score_control_alpha_rolls),
- ThetaSplineSegment("NeutralToMidCubeScore", neutral, neutral_to_score_1,
- neutral_to_score_2, score_mid_cube_pos,
- neutral_to_back_score_control_alpha_rolls),
- ThetaSplineSegment("NeutralToHighConeScore", neutral, neutral_to_score_1,
- neutral_to_score_2, score_high_cone_pos,
- neutral_to_back_score_control_alpha_rolls),
- ThetaSplineSegment("NeutralToHighCubeScore", neutral, neutral_to_score_1,
- neutral_to_score_2, score_high_cube_pos,
- neutral_to_back_score_control_alpha_rolls),
-]
-
segments = named_segments + unnamed_segments
diff --git a/y2023/control_loops/python/graph_tools.py b/y2023/control_loops/python/graph_tools.py
index 8751935..5033ccf 100644
--- a/y2023/control_loops/python/graph_tools.py
+++ b/y2023/control_loops/python/graph_tools.py
@@ -202,7 +202,8 @@
def get_circular_index(theta):
- return int(np.floor((theta[1] - theta[0]) / np.pi))
+ return int(
+ np.floor((shift_angle(theta[1]) - shift_angle(theta[0])) / np.pi))
def get_xy(theta):
@@ -447,6 +448,52 @@
self.alpha_unitizer = alpha_unitizer
self.vmax = vmax
+ def Print(self, points):
+ # Find the name of the start end end points.
+ start_name = None
+ end_name = None
+ for name in points:
+ point = points[name]
+ if (self.start == point[:2]).all():
+ start_name = name
+ elif (self.end == point[:2]).all():
+ end_name = name
+
+ alpha_points = '[' + ', '.join([
+ f"({alpha}, np.pi * {theta / np.pi})"
+ for alpha, theta in self.alpha_rolls[1:-1]
+ ]) + ']'
+
+ def FormatToTheta(point):
+ x, y = get_xy(point)
+ circular_index = get_circular_index(point)
+ return "to_theta_with_circular_index(%.3f, %.3f, circular_index=%d)" % (
+ x, y, circular_index)
+
+ def FormatToThetaRoll(point, roll):
+ x, y = get_xy(point)
+ circular_index = get_circular_index(point)
+ return "to_theta_with_circular_index_and_roll(%.3f, %.3f, np.pi * %.2f, circular_index=%d)" % (
+ x, y, roll / np.pi, circular_index)
+
+ print('named_segments.append(')
+ print(' ThetaSplineSegment(')
+ print(f' name="{self.name}",')
+ print(
+ f' start=points["{start_name}"], # {FormatToThetaRoll(self.start, self.alpha_rolls[0][1])}'
+ )
+ print(
+ f' control1=np.array([{self.control1[0]}, {self.control1[1]}]), # {FormatToTheta(self.control1)}'
+ )
+ print(
+ f' control2=np.array([{self.control2[0]}, {self.control2[1]}]), # {FormatToTheta(self.control2)}'
+ )
+ print(
+ f' end=points["{end_name}"], # {FormatToThetaRoll(self.end, self.alpha_rolls[-1][1])}'
+ )
+ print(f' control_alpha_rolls={alpha_points},')
+ print(f'))')
+
def __repr__(self):
return "ThetaSplineSegment(%s, %s, %s, %s)" % (repr(
self.start), repr(self.control1), repr(
diff --git a/y2023/control_loops/superstructure/arm/arm.cc b/y2023/control_loops/superstructure/arm/arm.cc
index fd265c4..03a1b21 100644
--- a/y2023/control_loops/superstructure/arm/arm.cc
+++ b/y2023/control_loops/superstructure/arm/arm.cc
@@ -38,7 +38,7 @@
search_graph_(MakeSearchGraph(&dynamics_, &trajectories_, alpha_unitizer_,
kVMax(), &hybrid_roll_joint_loop_)),
// Go to the start of the first trajectory.
- follower_(&dynamics_, &hybrid_roll_joint_loop_, NeutralPosPoint()),
+ follower_(&dynamics_, &hybrid_roll_joint_loop_, NeutralPoint()),
points_(PointList()),
current_node_(0) {
int i = 0;
@@ -92,7 +92,7 @@
}
// TODO(milind): should we default to the closest position?
- uint32_t filtered_goal = arm::NeutralPosIndex();
+ uint32_t filtered_goal = arm::NeutralIndex();
if (unsafe_goal != nullptr) {
filtered_goal = *unsafe_goal;
}
diff --git a/y2023/control_loops/superstructure/superstructure_lib_test.cc b/y2023/control_loops/superstructure/superstructure_lib_test.cc
index 0ecc5cf..801fc55 100644
--- a/y2023/control_loops/superstructure/superstructure_lib_test.cc
+++ b/y2023/control_loops/superstructure/superstructure_lib_test.cc
@@ -192,7 +192,7 @@
event_loop_->MakeFetcher<Status>("/superstructure")),
superstructure_output_fetcher_(
event_loop_->MakeFetcher<Output>("/superstructure")) {
- InitializeArmPosition(arm::NeutralPosPoint());
+ InitializeArmPosition(arm::NeutralPoint());
phased_loop_handle_ = event_loop_->AddPhasedLoop(
[this](int) {
// Skip this the first time.
@@ -447,7 +447,7 @@
*builder.fbb(), constants::Values::kWristRange().middle());
Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
- goal_builder.add_arm_goal_position(arm::NeutralPosIndex());
+ goal_builder.add_arm_goal_position(arm::NeutralIndex());
goal_builder.add_trajectory_override(false);
goal_builder.add_wrist(wrist_offset);
goal_builder.add_roller_goal(RollerGoal::IDLE);
@@ -473,7 +473,7 @@
Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
- goal_builder.add_arm_goal_position(arm::NeutralPosIndex());
+ goal_builder.add_arm_goal_position(arm::NeutralIndex());
goal_builder.add_trajectory_override(false);
goal_builder.add_wrist(wrist_offset);
goal_builder.add_roller_goal(RollerGoal::IDLE);
@@ -505,7 +505,7 @@
Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
goal_builder.add_wrist(wrist_offset);
- goal_builder.add_arm_goal_position(arm::NeutralPosIndex());
+ goal_builder.add_arm_goal_position(arm::NeutralIndex());
ASSERT_EQ(builder.Send(goal_builder.Finish()), aos::RawSender::Error::kOk);
}
@@ -526,7 +526,7 @@
goal_builder.add_wrist(wrist_offset);
- goal_builder.add_arm_goal_position(arm::NeutralPosIndex());
+ goal_builder.add_arm_goal_position(arm::NeutralIndex());
ASSERT_EQ(builder.Send(goal_builder.Finish()), aos::RawSender::Error::kOk);
}
@@ -582,7 +582,7 @@
Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
- goal_builder.add_arm_goal_position(arm::NeutralPosIndex());
+ goal_builder.add_arm_goal_position(arm::NeutralIndex());
goal_builder.add_trajectory_override(false);
goal_builder.add_roller_goal(GetParam() == GamePiece::CONE
? RollerGoal::INTAKE_CONE
@@ -623,7 +623,7 @@
Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
- goal_builder.add_arm_goal_position(arm::NeutralPosIndex());
+ goal_builder.add_arm_goal_position(arm::NeutralIndex());
goal_builder.add_trajectory_override(false);
goal_builder.add_roller_goal(RollerGoal::IDLE);
@@ -659,7 +659,7 @@
Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
- goal_builder.add_arm_goal_position(arm::NeutralPosIndex());
+ goal_builder.add_arm_goal_position(arm::NeutralIndex());
goal_builder.add_trajectory_override(false);
goal_builder.add_roller_goal(GetParam() == GamePiece::CONE
? RollerGoal::INTAKE_CONE
@@ -698,7 +698,7 @@
Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
- goal_builder.add_arm_goal_position(arm::NeutralPosIndex());
+ goal_builder.add_arm_goal_position(arm::NeutralIndex());
goal_builder.add_trajectory_override(false);
goal_builder.add_roller_goal(RollerGoal::SPIT);
@@ -736,7 +736,7 @@
Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
- goal_builder.add_arm_goal_position(arm::NeutralPosIndex());
+ goal_builder.add_arm_goal_position(arm::NeutralIndex());
goal_builder.add_trajectory_override(false);
goal_builder.add_roller_goal(RollerGoal::IDLE);
@@ -780,7 +780,7 @@
{
auto builder = superstructure_goal_sender_.MakeBuilder();
Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
- goal_builder.add_arm_goal_position(arm::NeutralPosIndex());
+ goal_builder.add_arm_goal_position(arm::NeutralIndex());
ASSERT_EQ(builder.Send(goal_builder.Finish()), aos::RawSender::Error::kOk);
}
@@ -803,7 +803,7 @@
TEST_F(SuperstructureTest, ArmMultistepMove) {
SetEnabled(true);
WaitUntilZeroed();
- superstructure_plant_.InitializeArmPosition(arm::NeutralPosPoint());
+ superstructure_plant_.InitializeArmPosition(arm::NeutralPoint());
{
auto builder = superstructure_goal_sender_.MakeBuilder();
@@ -819,7 +819,7 @@
{
auto builder = superstructure_goal_sender_.MakeBuilder();
Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
- goal_builder.add_arm_goal_position(arm::ScoreLowPosIndex());
+ goal_builder.add_arm_goal_position(arm::ConeDownPosIndex());
ASSERT_EQ(builder.Send(goal_builder.Finish()), aos::RawSender::Error::kOk);
}