Make zero straight up and fix all the positions.
This makes it so we can swap arms and everything works. Also, add a
scale factor to the distal to manange the difference in pulley sizes.
Change-Id: I974be95bf2c7493565ba9fe9c267e6e9a521f1b7
Signed-off-by: Austin Schuh <austin.linux@gmail.com>
diff --git a/y2023/control_loops/python/graph_paths.py b/y2023/control_loops/python/graph_paths.py
index 19e5a34..9aecb49 100644
--- a/y2023/control_loops/python/graph_paths.py
+++ b/y2023/control_loops/python/graph_paths.py
@@ -22,7 +22,7 @@
points['Neutral'] = np.array((np.pi, 0.0, 0.0))
points['GroundPickupBackConeUp'] = to_theta_with_circular_index_and_roll(
- -1.07774334, 0.40, np.pi / 2.0, circular_index=1)
+ -1.07774334, 0.39, np.pi / 2.0, circular_index=1)
named_segments.append(
ThetaSplineSegment(
@@ -35,7 +35,7 @@
))
points['GroundPickupBackConeDownBase'] = to_theta_with_circular_index_and_roll(
- -1.11487594, 0.25, np.pi / 2.0, circular_index=1)
+ -1.11487594, 0.24, np.pi / 2.0, circular_index=1)
named_segments.append(
ThetaSplineSegment(
@@ -49,7 +49,7 @@
points[
'GroundPickupFrontConeDownBase'] = to_theta_with_circular_index_and_roll(
- 0.30, 0.24, -np.pi / 2.0, circular_index=0)
+ 0.30, 0.265, -np.pi / 2.0, circular_index=0)
named_segments.append(
ThetaSplineSegment(
@@ -257,7 +257,7 @@
))
points['ScoreBackMidConeUp'] = to_theta_with_circular_index_and_roll(
- -1.45013, 1.04354, np.pi / 2.0, circular_index=1)
+ -1.45013, 1.00354, np.pi / 2.0, circular_index=1)
named_segments.append(
ThetaSplineSegment(
@@ -329,7 +329,7 @@
))
points['HPPickupBackConeUp'] = to_theta_with_circular_index_and_roll(
- -1.1200539, 1.345, np.pi / 2.0, circular_index=0)
+ -1.1200539, 1.330, np.pi / 2.0, circular_index=0)
named_segments.append(
ThetaSplineSegment(
@@ -344,7 +344,7 @@
))
points['HPPickupFrontConeUp'] = np.array(
- (5.16514378449353, 1.26, -np.pi / 2.0))
+ (5.16514378449353, 1.25, -np.pi / 2.0))
# to_theta_with_circular_index_and_roll(
# 0.265749, 1.28332, -np.pi / 2.0, circular_index=1)
@@ -361,7 +361,7 @@
))
points['ScoreFrontHighConeUp'] = to_theta_with_circular_index_and_roll(
- 0.98810344, 1.37536719, -np.pi / 2.0, circular_index=0)
+ 0.98810344, 1.31536719, -np.pi / 2.0, circular_index=0)
named_segments.append(
ThetaSplineSegment(
@@ -374,7 +374,7 @@
))
points['ScoreFrontMidConeUp'] = to_theta_with_circular_index_and_roll(
- 0.64, 1.03, -np.pi / 2.0, circular_index=0)
+ 0.64, 0.99, -np.pi / 2.0, circular_index=0)
named_segments.append(
ThetaSplineSegment(
@@ -536,7 +536,7 @@
))
points['GroundPickupFrontConeUp'] = to_theta_with_circular_index_and_roll(
- 0.313099, 0.380, -np.pi / 2.0, circular_index=0)
+ 0.313099, 0.390, -np.pi / 2.0, circular_index=0)
named_segments.append(
ThetaSplineSegment(