wip

axi/planner.py

@@ -97,6 +97,18 @@ class Piece(object):
     def velocity(self, t):
         return self.v1 + self.acceleration * t
 
+class JerkPiece(object):
+    # a constant jerk for a duration of time
+    def __init__(self, p1, p2, v1, a1, jerk, duration):
+        self.p1 = p1
+        self.p2 = p2
+        self.v1 = v1
+        self.v2 = v1 + a1 * duration + jerk * duration * duration / 2
+        self.a1 = a1
+        self.a2 = a1 + jerk * duration
+        self.jerk = jerk
+        self.duration = duration
+
 class Segment(object):
     # a segment is a line segment between two points, which will be broken
     # up into pieces by the planner
@@ -111,11 +123,11 @@ class Segment(object):
 
 class Planner(object):
     # a planner has a constant acceleration and a max crusing velocity
-    def __init__(self, acceleration, max_velocity, corner_factor, jerk):
+    def __init__(self, acceleration, max_velocity, corner_factor):
         self.acceleration = acceleration
         self.max_velocity = max_velocity
         self.corner_factor = corner_factor
-        self.jerk = jerk
+        self.jerk_factor = 0.5
 
     def plan(self, points):
         a = self.acceleration
@@ -205,15 +217,27 @@ class Planner(object):
         return result
 
     def smooth_group(self, pieces, a):
-        j = self.jerk
+        if abs(a) < EPS:
+            return pieces # TODO: convert to jerk pieces
         t = sum(x.duration for x in pieces)
-        vi = pieces[0].v1
+        # vi = pieces[0].v1
-        vf = pieces[-1].v2
+        # vf = pieces[-1].v2
-        print a, len(pieces), vi, vf, t
+        # s = (vf + vi) / 2 * t
-        # /|___|\
+        jf = self.jerk_factor
-        jf = 0.5
+        t1 = t * jf
-        t1 = t * j
+        t2 = t - 2 * t1
-        t2 = t * (1 - j)
+        amax = a / (1 - jf)
+        jerk = amax / t1
+        # jerk for t1, a = 0 to amax
+        # accel for t2, a = amax
+        # -jerk for t1, a = amax to 0
+        # blocks = [
+        #     JerkBlock(0, jerk, t1),
+        #     JerkBlock(amax, jerk, t2),
+        #     JerkBlock(0, jerk, t1),
+        # ]
+        # s = vi * t + ai * t * t / 2 + j * t * t * t / 6
+        print a, len(pieces), t, jerk, amax
         return pieces
 
 # vf = vi + a * t
@@ -221,21 +245,25 @@ class Planner(object):
 # s = vi * t + a * t * t / 2
 # vf * vf = vi * vi + 2 * a * s
 
-def chop_piece(p, dt):
+# af = ai + j * t
-    result = []
+# vf = vi + ai * t + j * t * t / 2
-    t = 0
+# sf = si + vi * t + ai * t * t / 2 + j * t * t * t / 6
-    while t < p.duration:
-        t1 = t
-        t2 = min(t + dt, p.duration)
-        p1 = p.point(t1)
-        p2 = p.point(t2)
-        v = (p.velocity(t1) + p.velocity(t2)) / 2
-        result.append(Piece(p1, p2, v, 0, t2 - t1))
-        t += dt
-    return result
 
-def chop_pieces(pieces, dt):
+# def chop_piece(p, dt):
-    result = []
+#     result = []
-    for piece in pieces:
+#     t = 0
-        result.extend(chop_piece(piece, dt))
+#     while t < p.duration:
-    return result
+#         t1 = t
+#         t2 = min(t + dt, p.duration)
+#         p1 = p.point(t1)
+#         p2 = p.point(t2)
+#         v = (p.velocity(t1) + p.velocity(t2)) / 2
+#         result.append(Piece(p1, p2, v, 0, t2 - t1))
+#         t += dt
+#     return result
+
+# def chop_pieces(pieces, dt):
+#     result = []
+#     for piece in pieces:
+#         result.extend(chop_piece(piece, dt))
+#     return result

jerk_test.py

@@ -0,0 +1,30 @@
+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=50, max_velocity=200, corner_factor=1)
+    pieces = planner.plan(points)
+    # print 'var PIECES = ['
+    # for p in pieces:
+    #     record = (p.p1.x, p.p1.y, p.p2.x, p.p2.y, p.acceleration, p.duration)
+    #     print '[%s],' % ','.join(map(str, record))
+    # print '];'
+    pieces = planner.smooth(pieces)
+    # for p in pieces:
+    #     print p.acceleration, p.duration
+
+if __name__ == '__main__':
+    main()

planner_test.py

@@ -15,7 +15,7 @@ def main():
     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=50, max_velocity=200, corner_factor=1, jerk=100)
+    planner = Planner(acceleration=50, max_velocity=200, corner_factor=1)
     pieces = planner.plan(points)
     print 'var PIECES = ['
     for p in pieces: