commit 68842e152b4e165c43a0f590499f6b8e4f38994a
author milind-u <milind.upadhyay@gmail.com> Sun Feb 26 12:45:40 2023 -0800
committer Austin Schuh <austin.linux@gmail.com> Sun Feb 26 14:14:28 2023 -0800
tree 03b08ba66307807e12507d912fa9b0716fc6ac5f
parent a750835968b39372e5b1f800724198068c1e5958

Add cube pickup path

Turns out the link lengths in python didn't match C++...  Sigh.  I
updated all the trajectories to be equivilent to what they were before
the constant switch.

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

y2023/control_loops/python/graph_tools.py

diff --git a/y2023/control_loops/python/graph_tools.py b/y2023/control_loops/python/graph_tools.py
index 769c7b3..8751935 100644
--- a/y2023/control_loops/python/graph_tools.py
+++ b/y2023/control_loops/python/graph_tools.py
@@ -9,7 +9,7 @@
 
 # Joint distances (l1 = "proximal", l2 = "distal")
 l1 = 20.0 * IN_TO_M
-l2 = 31.5 * IN_TO_M
+l2 = 38.0 * IN_TO_M
 
 max_dist = 0.01
 max_dist_theta = np.pi / 64
@@ -35,7 +35,7 @@
     orient = (circular_index % 2) == 0
     x = pt[0]
     y = pt[1]
-    x -= joint_center[0]
+    x -= joint_center[0] - 1e-9
     y -= joint_center[1]
     l3 = np.hypot(x, y)
     t3 = np.arctan2(y, x)