Show robot along spline on hover in Spline UI
This makes it much easier to tell if the robot is going to hit something
somewhere along the path.
Change-Id: Ib2cadc3cc9aa6faad9098476e76487e7462630a0
Signed-off-by: Ravago Jones <ravagojones@gmail.com>
diff --git a/frc971/control_loops/python/points.py b/frc971/control_loops/python/points.py
index dd7e199..868885a 100644
--- a/frc971/control_loops/python/points.py
+++ b/frc971/control_loops/python/points.py
@@ -1,5 +1,6 @@
from constants import *
import numpy as np
+import scipy.optimize
from libspline import Spline, DistanceSpline, Trajectory
import copy
@@ -119,6 +120,40 @@
spline = Spline(np.ascontiguousarray(np.transpose(array)))
self.libsplines.append(spline)
+ def nearest_distance(self, point):
+ """Finds the distance along the DistanceSpline that is closest to the
+ given point on the field"""
+ def distance(t, distance_spline, point):
+ return np.sum((distance_spline.XY(t) - point)**2)
+
+ # We know the derivative of the function,
+ # so scipy doesn't need to compute it every time
+ def ddistance(t, distance_spline, point):
+ return np.sum(2 * (distance_spline.XY(t) - point) *
+ distance_spline.DXY(t))
+
+ distance_spline = DistanceSpline(self.getLibsplines())
+ best_result = None
+
+ # The optimizer finds local minima that often aren't what we want,
+ # so try from multiple locations to find a better minimum.
+ guess_points = np.linspace(0, distance_spline.Length(), num=5)
+
+ for guess in guess_points:
+ result = scipy.optimize.minimize(
+ distance,
+ guess,
+ args=(distance_spline, point),
+ bounds=((0, distance_spline.Length()), ),
+ jac=ddistance,
+ )
+
+ if result.success and (best_result == None
+ or result.fun < best_result.fun):
+ best_result = result
+
+ return best_result, distance_spline
+
def toMultiSpline(self):
multi_spline = {
"spline_count": 0,