Make the paths followable and not hit anything
Change-Id: Ib601354ee229270981b10f44747d55176f672c02
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 b21f548..2a5ef73 100644
--- a/y2023/control_loops/python/graph_paths.py
+++ b/y2023/control_loops/python/graph_paths.py
@@ -6,22 +6,23 @@
joint_center[1] + l2 - l1,
0.0,
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)
+neutral_to_pickup_1 = np.array([2.396694, 0.508020])
+neutral_to_pickup_2 = np.array([2.874513, 0.933160])
pickup_pos = to_theta_with_circular_index_and_roll(0.6,
- 0.1,
- 0.0,
- circular_index=1)
-neutral_to_pickup_control_alpha_rolls = [(0.33, 0.0), (.67, 0.0)]
+ 0.4,
+ np.pi / 2.0,
+ circular_index=0)
-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)
+neutral_to_pickup_control_alpha_rolls = [(0.33, 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_pos = to_theta_with_circular_index_and_roll(-1.0,
1.2,
- 0.0,
- circular_index=1)
-neutral_to_score_control_alpha_rolls = [(0.33, 0.0), (.67, 0.0)]
+ np.pi / 2.0,
+ circular_index=0)
+neutral_to_score_control_alpha_rolls = [(0.33, 0.0), (.95, np.pi / 2.0)]
# TODO(Max): Add real paths for arm.
points = [(neutral, "NeutralPos"), (pickup_pos, "PickupPos"),