From b0dc60fec027fc0a669e54796f8d441480558428 Mon Sep 17 00:00:00 2001
From: Michael Fogleman <fogleman@gmail.com>
Date: Mon, 12 Mar 2018 22:05:28 -0400
Subject: [PATCH] add throttler to planner

---
 axi/planner.py | 80 ++++++++++++++++++++++++++++++++++++++++++++++++--
 1 file changed, 77 insertions(+), 3 deletions(-)

diff --git a/axi/planner.py b/axi/planner.py
index 62a712b..b8912b0 100644
--- a/axi/planner.py
+++ b/axi/planner.py
@@ -90,6 +90,9 @@ class Point(_Point):
     def distance(self, other):
         return hypot(self.x - other.x, self.y - other.y)
 
+    def distance_squared(self, other):
+        return (self.x - other.x) ** 2 + (self.y - other.y) ** 2
+
     def add(self, other):
         return Point(self.x + other.x, self.y + other.y)
 
@@ -106,6 +109,18 @@ class Point(_Point):
         v = other.sub(self).normalize()
         return self.add(v.mul(s))
 
+    def segment_distance(self, v, w):
+        p = self
+        l2 = v.distance_squared(w)
+        if l2 == 0:
+            return p.distance(v)
+        t = ((p.x - v.x) * (w.x - v.x) + (p.y - v.y) * (w.y - v.y)) / l2
+        t = max(0, min(1, t))
+        x = v.x + t * (w.x - v.x)
+        y = v.y + t * (w.y - v.y)
+        q = Point(x, y)
+        return p.distance(q)
+
 Triangle = namedtuple('Triangle',
     ['s1', 's2', 't1', 't2', 'vmax', 'p1', 'p2', 'p3'])
 
@@ -158,18 +173,77 @@ class Segment(object):
         self.entry_velocity = 0
         self.blocks = []
 
+class Throttler(object):
+    def __init__(self, points, vmax, dt, threshold):
+        self.points = points
+        self.vmax = vmax
+        self.dt = dt
+        self.threshold = threshold
+        self.distances = []
+        prev = points[0]
+        d = 0
+        for point in points:
+            d += prev.distance(point)
+            self.distances.append(d)
+            prev = point
+    def lookup(self, d):
+        return bisect(self.distances, d) - 1
+    def is_feasible(self, i0, v):
+        d = v * self.dt
+        x0 = self.distances[i0]
+        x1 = x0 + d
+        i1 = self.lookup(x1)
+        if i0 == i1:
+            return True
+        p0 = self.points[i0]
+        p10 = self.points[i1]
+        try:
+            p11 = self.points[i1+1]
+        except IndexError:
+            p11 = p10
+        s = x1 - self.distances[i1]
+        p1 = p10.lerps(p11, s)
+        i = i0 + 1
+        while i <= i1:
+            p = self.points[i]
+            if p.segment_distance(p0, p1) > self.threshold:
+                return False
+            i += 1
+        return True
+    def compute_max_velocity(self, index):
+        if self.is_feasible(index, self.vmax):
+            return self.vmax
+        lo = 0
+        hi = self.vmax
+        for _ in range(16):
+            v = (lo + hi) / 2
+            if self.is_feasible(index, v):
+                lo = v
+            else:
+                hi = v
+        v = lo
+        return v
+    def compute_max_velocities(self):
+        return [self.compute_max_velocity(i) for i in range(len(self.points))]
+
 def constant_acceleration_plan(points, a, vmax, cf):
     # make sure points are Point objects
     points = [Point(x, y) for x, y in points]
 
+    # the throttler reduces speeds based on the discrete timeslicing nature of
+    # the device
+    # TODO: expose parameters
+    throttler = Throttler(points, vmax, 0.02, 0.001)
+    max_velocities = throttler.compute_max_velocities()
+
     # create segments for each consecutive pair of points
     segments = [Segment(p1, p2) for p1, p2 in zip(points, points[1:])]
 
     # compute a max_entry_velocity for each segment
     # based on the angle formed by the two segments at the vertex
-    for s1, s2 in zip(segments, segments[1:]):
-        v = corner_velocity(s1, s2, vmax, a, cf)
-        s2.max_entry_velocity = v
+    for v, s1, s2 in zip(max_velocities, segments, segments[1:]):
+        s1.max_entry_velocity = min(s1.max_entry_velocity, v)
+        s2.max_entry_velocity = corner_velocity(s1, s2, vmax, a, cf)
 
     # add a dummy segment at the end to force a final velocity of zero
     segments.append(Segment(points[-1], points[-1]))