From ca71918437a219694060787d7e4f2999a46d0d16 Mon Sep 17 00:00:00 2001
From: Michael Fogleman <fogleman@gmail.com>
Date: Fri, 6 Jan 2017 16:35:27 -0500
Subject: [PATCH] remove jerk

---
 axi/planner.py    | 285 ++++++++++++++--------------------------------
 butterfly_test.py |   6 +-
 jerk_test.py      |  28 -----
 planner_test.py   |   4 +-
 4 files changed, 89 insertions(+), 234 deletions(-)
 delete mode 100644 jerk_test.py

diff --git a/axi/planner.py b/axi/planner.py
index 88610ff..51245c9 100644
--- a/axi/planner.py
+++ b/axi/planner.py
@@ -2,9 +2,73 @@ from __future__ import division
 
 from bisect import bisect
 from collections import namedtuple
-from itertools import groupby
 from math import sqrt, hypot
-import numpy
+
+# a planner computes a motion profile for a list of (x, y) points
+class Planner(object):
+    def __init__(self, acceleration, max_velocity, corner_factor):
+        self.acceleration = acceleration
+        self.max_velocity = max_velocity
+        self.corner_factor = corner_factor
+
+    def plan(self, points):
+        return constant_acceleration_plan(
+            points, self.acceleration, self.max_velocity, self.corner_factor)
+
+# a plan is a motion profile generated by the planner
+class Plan(object):
+    def __init__(self, blocks):
+        self.blocks = blocks
+        self.ts = [] # start time of each block
+        self.ss = [] # start distance of each block
+        t = 0
+        s = 0
+        for b in blocks:
+            self.ts.append(t)
+            self.ss.append(s)
+            t += b.t
+            s += b.s
+        self.t = t # total time
+        self.s = s # total duration
+
+    def instant(self, t):
+        t = max(0, min(self.t, t)) # clamp t
+        i = bisect(self.ts, t) - 1 # find block for t
+        return self.blocks[i].instant(t - self.ts[i], self.ts[i], self.ss[i])
+
+# a block is a constant acceleration for a duration of time
+class Block(object):
+    def __init__(self, a, t, vi, p1, p2):
+        self.a = a
+        self.t = t
+        self.vi = vi
+        self.p1 = p1
+        self.p2 = p2
+        self.s = p1.distance(p2)
+
+    def instant(self, t, dt=0, ds=0):
+        t = max(0, min(self.t, t)) # clamp t
+        a = self.a
+        v = self.vi + self.a * t
+        s = self.vi * t + self.a * t * t / 2
+        s = max(0, min(self.s, s)) # clamp s
+        p = self.p1.lerps(self.p2, s)
+        return Instant(t + dt, p, s + ds, v, a)
+
+# an instant gives position, velocity, etc. at a single point in time
+Instant = namedtuple('Instant', ['t', 'p', 's', 'v', 'a'])
+
+# a = acceleration
+# v = velocity
+# s = distance
+# t = time
+# i = initial
+# f = final
+
+# vf = vi + a * t
+# s = (vf + vi) / 2 * t
+# s = vi * t + a * t * t / 2
+# vf * vf = vi * vi + 2 * a * s
 
 EPS = 1e-9
 
@@ -67,28 +131,6 @@ def trapezoid(s, vi, vmax, vf, a, p1, p4):
     p3 = p1.lerps(p4, s - s3)
     return Trapezoid(s1, s2, s3, t1, t2, t3, p1, p2, p3, p4)
 
-def acceleration_duration(s, vi, a):
-    # compute the amount of time to travel distance s while accelerating
-    vf = sqrt(vi * vi + 2 * a * s)
-    t = (vf - vi) / a
-    return t
-
-def jerk_duration(s, vi, ai, j):
-    # compute the amount of time to travel distance s while jerking
-    # TODO: remove numpy dependency?
-    roots = numpy.roots([j / 6, ai / 2, vi, -s])
-    roots = roots.real[abs(roots.imag) < EPS]
-    return float(min(x for x in roots if x > 0))
-
-def jerk_factor(a, j, t):
-    # compute a jerk factor based on desired jerk, 0 < jf <= 0.5
-    a = abs(a)
-    jt = j * t
-    r = jt * (jt - 4 * a)
-    if r < EPS:
-        return 0.5
-    return (jt - sqrt(r)) / (2 * jt)
-
 def corner_velocity(s1, s2, vmax, a, delta):
     # compute a maximum velocity at the corner of two segments
     # https://onehossshay.wordpress.com/2011/09/24/improving_grbl_cornering_algorithm/
@@ -101,63 +143,6 @@ def corner_velocity(s1, s2, vmax, a, delta):
     v = sqrt((a * delta * sine) / (1 - sine))
     return min(v, vmax)
 
-Instant = namedtuple('Instant', ['t', 'p', 's', 'v', 'a', 'j'])
-
-class Plan(object):
-    # a complete motion profile
-    def __init__(self, blocks):
-        self.blocks = blocks
-        self.duration = sum(b.t for b in blocks)
-        self.length = sum(b.s for b in blocks)
-        self.times = [] # start time of each block
-        t = 0
-        for b in blocks:
-            self.times.append(t)
-            t += b.t
-
-    def instant(self, t):
-        t = max(0, t)
-        i = bisect(self.times, t) - 1
-        b = self.blocks[i]
-        bt = t - self.times[i]
-        return b.instant(bt)
-
-class Block(object):
-    # a constant jerk for a duration of time
-    def __init__(self, j, t, vi, ai, p1, p2):
-        # TODO: track total time and distance for entire path or do in post?
-        self.j = j
-        self.t = t
-        self.vi = vi
-        self.ai = ai
-        self.p1 = p1 # TODO: rename pi?
-        self.p2 = p2 # TODO: rename pf?
-        self.s = p1.distance(p2)
-        # TODO: support providing vf, af when known
-        self.vf = vi + ai * t + j * t * t / 2
-        self.af = ai + j * t
-
-    def split(self, t):
-        x = self.instant(t)
-        b1 = Block(self.j, t, self.vi, self.ai, self.p1, x.p)
-        b2 = Block(self.j, self.t - t, x.v, x.a, x.p, self.p2)
-        return b1, b2
-
-    def instant(self, t):
-        t2 = t * t
-        t3 = t2 * t
-        t2_2 = t2 / 2
-        t3_6 = t3 / 6
-        j = self.j
-        a = self.ai + self.j * t
-        v = self.vi + self.ai * t + self.j * t2_2
-        s = self.vi * t + self.ai * t2_2 + self.j * t3_6
-        p = self.p1.lerps(self.p2, s)
-        return Instant(t, p, s, v, a, j)
-
-def accelerate(a, t, vi, p1, p2):
-    return Block(0, t, vi, a, p1, p2)
-
 class Segment(object):
     # a segment is a line segment between two points, which will be broken
     # up into blocks by the planner
@@ -170,23 +155,6 @@ class Segment(object):
         self.entry_velocity = 0
         self.blocks = []
 
-class Planner(object):
-    def __init__(self, acceleration, max_velocity, corner_factor, jerk):
-        self.acceleration = acceleration
-        self.max_velocity = max_velocity
-        self.corner_factor = corner_factor
-        self.jerk = jerk
-
-    def plan(self, points):
-        a = self.acceleration
-        vmax = self.max_velocity
-        cf = self.corner_factor
-        return constant_acceleration_plan(points, a, vmax, cf)
-
-    def jerk_plan(self, points):
-        plan = self.plan(points)
-        return constant_jerk_plan(plan, self.jerk)
-
 def constant_acceleration_plan(points, a, vmax, cf):
     # make sure points are Point objects
     points = [Point(x, y) for x, y in points]
@@ -216,48 +184,38 @@ def constant_acceleration_plan(points, a, vmax, cf):
         p2 = segment.p2
 
         # determine which profile to use for this segment
-        # TODO: rearrange these cases for better flow?
-
-        # TODO: ensure acceleration blocks are long enough to jerk
-        # min_acceleration_duration = 2 * a / j
-
-        # accelerate? /
-        vf = sqrt(vi * vi + 2 * a * s)
-        if vf <= vexit:
+        m = triangle(s, vi, vexit, a, p1, p2)
+        if m.s1 < -EPS:
+            # too fast! update max_entry_velocity and backtrack
+            segment.max_entry_velocity = sqrt(vexit * vexit + 2 * a * s)
+            i -= 1
+        elif m.s2 < 0:
+            # accelerate
+            vf = sqrt(vi * vi + 2 * a * s)
             t = (vf - vi) / a
             segment.blocks = [
-                accelerate(a, t, vi, p1, p2),
+                Block(a, t, vi, p1, p2),
             ]
             next_segment.entry_velocity = vf
             i += 1
-            continue
-
-        # accelerate, cruise, decelerate? /---\
-        m = triangle(s, vi, vexit, a, p1, p2)
-        if m.s1 > -EPS and m.s2 > -EPS and m.vmax >= vmax:
+        elif m.vmax > vmax:
+            # accelerate, cruise, decelerate
             z = trapezoid(s, vi, vmax, vexit, a, p1, p2)
             segment.blocks = [
-                accelerate(a, z.t1, vi, z.p1, z.p2),
-                accelerate(0, z.t2, vmax, z.p2, z.p3),
-                accelerate(-a, z.t3, vmax, z.p3, z.p4),
+                Block(a, z.t1, vi, z.p1, z.p2),
+                Block(0, z.t2, vmax, z.p2, z.p3),
+                Block(-a, z.t3, vmax, z.p3, z.p4),
             ]
             next_segment.entry_velocity = vexit
             i += 1
-            continue
-
-        # accelerate, decelerate? /\
-        if m.s1 > -EPS and m.s2 > -EPS:
+        else:
+            # accelerate, decelerate
             segment.blocks = [
-                accelerate(a, m.t1, vi, m.p1, m.p2),
-                accelerate(-a, m.t2, m.vmax, m.p2, m.p3),
+                Block(a, m.t1, vi, m.p1, m.p2),
+                Block(-a, m.t2, m.vmax, m.p2, m.p3),
             ]
             next_segment.entry_velocity = vexit
             i += 1
-            continue
-
-        # too fast! update max_entry_velocity and backtrack
-        segment.max_entry_velocity = sqrt(vexit * vexit + 2 * a * s)
-        i -= 1 # TODO: support non-zero initial velocity?
 
     # concatenate all of the blocks
     blocks = []
@@ -267,78 +225,3 @@ def constant_acceleration_plan(points, a, vmax, cf):
     # filter out zero-duration blocks and return
     blocks = [b for b in blocks if b.t > EPS]
     return Plan(blocks)
-
-def constant_jerk_plan(plan, j):
-    # TODO: ignore blocks that already have a jerk?
-    blocks = []
-    for a, g in groupby(plan.blocks, key=lambda b: b.ai):
-        blocks.extend(_constant_jerk_plan(list(g), j, a))
-    return Plan(blocks)
-
-def _constant_jerk_plan(blocks, j, a):
-    if abs(a) < EPS:
-        return blocks
-    result = []
-    duration = sum(b.t for b in blocks)
-    jf = jerk_factor(a, j, duration)
-    t1 = duration * jf
-    t2 = duration - 2 * t1
-    amax = a / (1 - jf)
-    j = amax / t1 # actual jerk may exceed desired jerk
-    vi = blocks[0].vi
-    ai = 0
-    s1 = vi * t1 + ai * t1 * t1 / 2 + j * t1 * t1 * t1 / 6
-    v1 = vi + ai * t1 + j * t1 * t1 / 2
-    s2 = v1 * t2 + amax * t2 * t2 / 2
-    blocks1, temp = split_blocks(blocks, s1)
-    blocks2, blocks3 = split_blocks(temp, s2)
-    # jerk to a = amax
-    for b in blocks1:
-        t = jerk_duration(b.s, vi, ai, j)
-        block = Block(j, t, vi, ai, b.p1, b.p2)
-        result.append(block)
-        vi = block.vf
-        ai = block.af
-    # accelerate at amax
-    for b in blocks2:
-        t = acceleration_duration(b.s, vi, ai)
-        block = Block(0, t, vi, ai, b.p1, b.p2)
-        result.append(block)
-        vi = block.vf
-        ai = block.af
-    # jerk to a = 0
-    for b in blocks3:
-        t = jerk_duration(b.s, vi, ai, -j)
-        block = Block(-j, t, vi, ai, b.p1, b.p2)
-        result.append(block)
-        vi = block.vf
-        ai = block.af
-    return result
-
-def split_blocks(blocks, s):
-    before = []
-    after = []
-    total = 0
-    for b in blocks:
-        s1 = total
-        s2 = total + b.s
-        if s2 < s + EPS:
-            before.append(b)
-        elif s1 > s - EPS:
-            after.append(b)
-        else:
-            t = acceleration_duration(s - s1, b.vi, b.ai)
-            b1, b2 = b.split(t)
-            before.append(b1)
-            after.append(b2)
-        total = s2
-    return before, after
-
-# vf = vi + a * t
-# s = (vf + vi) / 2 * t
-# s = vi * t + a * t * t / 2
-# vf * vf = vi * vi + 2 * a * s
-
-# af = ai + j * t
-# vf = vi + ai * t + j * t * t / 2
-# sf = si + vi * t + ai * t * t / 2 + j * t * t * t / 6
diff --git a/butterfly_test.py b/butterfly_test.py
index 3221563..f0bc2e8 100644
--- a/butterfly_test.py
+++ b/butterfly_test.py
@@ -2,7 +2,7 @@ from axi import Planner
 
 def main():
     planner = Planner(
-        acceleration=100, max_velocity=200, corner_factor=0.1, jerk=5000)
+        acceleration=100, max_velocity=200, corner_factor=0.1)
     draws = list(PATHS)
     jogs = []
     for p1, p2 in zip(draws, draws[1:]):
@@ -13,9 +13,9 @@ def main():
     print 'var PIECES = ['
     for i, path in enumerate(paths):
         print '['
-        plan = planner.jerk_plan(path)
+        plan = planner.plan(path)
         for b in plan.blocks:
-            record = (b.p1.x, b.p1.y, b.p2.x, b.p2.y, b.j, b.t, i)
+            record = (b.p1.x, b.p1.y, b.p2.x, b.p2.y, b.a, b.t, i)
             print '[%s],' % ','.join(map(str, record))
         print '],'
     print '];'
diff --git a/jerk_test.py b/jerk_test.py
deleted file mode 100644
index 1848fa3..0000000
--- a/jerk_test.py
+++ /dev/null
@@ -1,28 +0,0 @@
-from axi import Planner
-from math import pi, sin, cos
-
-def circle(cx, cy, r, n):
-    points = []
-    for i in range(n + 1):
-        a = 2 * pi * i / n
-        x = cx + cos(a) * r
-        y = cy + sin(a) * r
-        points.append((x, y))
-    return points
-
-def main():
-    points = circle(0, 0, 100, 90)
-    points = [(-100, -100), (100, -100)] + points + [(100, 100), (-100, 100), (-100, -100)]
-    for r in range(20, 100, 20):
-        points = circle(0, 0, r, 90) + points
-    planner = Planner(
-        acceleration=100, max_velocity=200, corner_factor=1, jerk=5000)
-    plan = planner.jerk_plan(points)
-    print 'var PIECES = ['
-    for b in plan.blocks:
-        record = (b.p1.x, b.p1.y, b.p2.x, b.p2.y, b.j, b.t)
-        print '[%s],' % ','.join(map(str, record))
-    print '];'
-
-if __name__ == '__main__':
-    main()
diff --git a/planner_test.py b/planner_test.py
index ac145dd..df8210d 100644
--- a/planner_test.py
+++ b/planner_test.py
@@ -16,11 +16,11 @@ def main():
     for r in range(20, 100, 20):
         points = circle(0, 0, r, 90) + points
     planner = Planner(
-        acceleration=50, max_velocity=200, corner_factor=1, jerk=5000)
+        acceleration=50, max_velocity=200, corner_factor=1)
     plan = planner.plan(points)
     print 'var PIECES = ['
     for b in plan.blocks:
-        record = (b.p1.x, b.p1.y, b.p2.x, b.p2.y, b.ai, b.t)
+        record = (b.p1.x, b.p1.y, b.p2.x, b.p2.y, b.a, b.t)
         print '[%s],' % ','.join(map(str, record))
     print '];'