mattmcw
/
axi
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

remove jerk

This commit is contained in:
Michael Fogleman 2017-01-06 16:35:27 -05:00
parent e31a9cfa1e
commit ca71918437
4 changed files with 89 additions and 234 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

285
axi/planner.py
View File

@ -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

6
butterfly_test.py
View File

@ -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 '];'

28
jerk_test.py
View File

@ -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()

4
planner_test.py
View File

@ -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 '];'