commit 19a55e00a602e2a5db23eae482bb414a1877448c
author Austin Schuh <austin.linux@gmail.com> Sun Apr 16 17:54:35 2023 -0700
committer Naman Gupta <naman.gupta@bluerivertech.com> Tue Apr 25 22:53:21 2023 -0700
tree 29a688198a6485fd08637dfafdd14cfe6ca93dfe
parent 3a8e7e0d7a8be087844f89d03c6a44377969bb88

Tune arm splines to be faster

Change-Id: Iacdfbbd735d862399e26eb7e20301ffb60b62ddc
Signed-off-by: Austin Schuh <austin.linux@gmail.com>

y2023/control_loops/python/graph_paths.py

diff --git a/y2023/control_loops/python/graph_paths.py b/y2023/control_loops/python/graph_paths.py
index 1efcd93..065cae4 100644
--- a/y2023/control_loops/python/graph_paths.py
+++ b/y2023/control_loops/python/graph_paths.py
@@ -241,6 +241,8 @@
         control2=np.array([2.972776, -1.026820]),
         end=points['GroundPickupBackCube'],
         control_alpha_rolls=[(.9, -np.pi / 2.0)],
+        alpha_unitizer=np.matrix(
+            f"{1.0 / 20.0} 0 0; 0 {1.0 / 15.0} 0; 0 0 {1.0 / 90.0}"),
     ))
 
 points['GroundPickupFrontCube'] = to_theta_with_circular_index_and_roll(
@@ -254,6 +256,8 @@
         control2=np.array([4.28246270189025, 1.492916470137478]),
         end=points['GroundPickupFrontCube'],
         control_alpha_rolls=[(.9, np.pi / 2.0)],
+        alpha_unitizer=np.matrix(
+            f"{1.0 / 20.0} 0 0; 0 {1.0 / 15.0} 0; 0 0 {1.0 / 90.0}"),
     ))
 
 points['ScoreBackMidConeUp'] = to_theta_with_circular_index_and_roll(
@@ -334,13 +338,11 @@
 named_segments.append(
     ThetaSplineSegment(
         name="NeutralToHPPickupBackConeUp",
-        start=points['Neutral'],
-        control1=np.array([2.7014360412658567, -0.32490272351246796]),
-        control2=np.array([0.8282769211604871, -1.8026615176254461]),
-        end=points['HPPickupBackConeUp'],
-        control_alpha_rolls=[(.9, np.pi / 2.0)],
-        alpha_unitizer=np.matrix(
-            f"{1.0 / 15.0} 0 0; 0 {1.0 / 15.0} 0; 0 0 {1.0 / 100.0}"),
+        start=points["Neutral"],
+        control1=np.array([2.4081612851752787, -0.4499932641262525]),
+        control2=np.array([1.5966343156908454, -1.4332589701944083]),
+        end=points["HPPickupBackConeUp"],
+        control_alpha_rolls=[(0.9, np.pi * 0.5)],
     ))
 
 points['HPPickupFrontConeUp'] = np.array(