Fix arm UI spline editing
- Fix angle inversion when clicking
- Display spline handles
Signed-off-by: milind-u <milind.upadhyay@gmail.com>
Change-Id: Ib1ee3415e1a6c975df0aa8c0eb50479413831005
diff --git a/y2023/control_loops/python/graph_tools.py b/y2023/control_loops/python/graph_tools.py
index 4083906..7614390 100644
--- a/y2023/control_loops/python/graph_tools.py
+++ b/y2023/control_loops/python/graph_tools.py
@@ -31,7 +31,7 @@
# where circular_index is the circular index, or the position in the
# "hyperextension" zones. "cross_point" allows shifting the place where
# it rounds the result so that it draws nicer (no other functional differences).
-def to_theta(pt, circular_index, cross_point=-np.pi):
+def to_theta(pt, circular_index, cross_point=-np.pi, die=True):
orient = (circular_index % 2) == 0
x = pt[0]
y = pt[1]
@@ -41,8 +41,11 @@
t3 = np.arctan2(y, x)
theta1 = np.arccos((l1**2 + l3**2 - l2**2) / (2 * l1 * l3))
if np.isnan(theta1):
- traceback.print_stack()
- sys.exit("Couldn't fit triangle to %f, %f, %f" % (l1, l2, l3))
+ print(("Couldn't fit triangle to %f, %f, %f" % (l1, l2, l3)))
+ if die:
+ traceback.print_stack()
+ sys.exit(1)
+ return None
if orient:
theta1 = -theta1