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

View File

@ -2,9 +2,73 @@ from __future__ import division
from bisect import bisect from bisect import bisect
from collections import namedtuple from collections import namedtuple
from itertools import groupby
from math import sqrt, hypot 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 EPS = 1e-9
@ -67,28 +131,6 @@ def trapezoid(s, vi, vmax, vf, a, p1, p4):
p3 = p1.lerps(p4, s - s3) p3 = p1.lerps(p4, s - s3)
return Trapezoid(s1, s2, s3, t1, t2, t3, p1, p2, p3, p4) 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): def corner_velocity(s1, s2, vmax, a, delta):
# compute a maximum velocity at the corner of two segments # compute a maximum velocity at the corner of two segments
# https://onehossshay.wordpress.com/2011/09/24/improving_grbl_cornering_algorithm/ # 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)) v = sqrt((a * delta * sine) / (1 - sine))
return min(v, vmax) 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): class Segment(object):
# a segment is a line segment between two points, which will be broken # a segment is a line segment between two points, which will be broken
# up into blocks by the planner # up into blocks by the planner
@ -170,23 +155,6 @@ class Segment(object):
self.entry_velocity = 0 self.entry_velocity = 0
self.blocks = [] 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): def constant_acceleration_plan(points, a, vmax, cf):
# make sure points are Point objects # make sure points are Point objects
points = [Point(x, y) for x, y in points] points = [Point(x, y) for x, y in points]
@ -216,48 +184,38 @@ def constant_acceleration_plan(points, a, vmax, cf):
p2 = segment.p2 p2 = segment.p2
# determine which profile to use for this segment # determine which profile to use for this segment
# TODO: rearrange these cases for better flow? m = triangle(s, vi, vexit, a, p1, p2)
if m.s1 < -EPS:
# TODO: ensure acceleration blocks are long enough to jerk # too fast! update max_entry_velocity and backtrack
# min_acceleration_duration = 2 * a / j segment.max_entry_velocity = sqrt(vexit * vexit + 2 * a * s)
i -= 1
# accelerate? / elif m.s2 < 0:
# accelerate
vf = sqrt(vi * vi + 2 * a * s) vf = sqrt(vi * vi + 2 * a * s)
if vf <= vexit:
t = (vf - vi) / a t = (vf - vi) / a
segment.blocks = [ segment.blocks = [
accelerate(a, t, vi, p1, p2), Block(a, t, vi, p1, p2),
] ]
next_segment.entry_velocity = vf next_segment.entry_velocity = vf
i += 1 i += 1
continue elif m.vmax > vmax:
# accelerate, cruise, decelerate
# accelerate, cruise, decelerate? /---\
m = triangle(s, vi, vexit, a, p1, p2)
if m.s1 > -EPS and m.s2 > -EPS and m.vmax >= vmax:
z = trapezoid(s, vi, vmax, vexit, a, p1, p2) z = trapezoid(s, vi, vmax, vexit, a, p1, p2)
segment.blocks = [ segment.blocks = [
accelerate(a, z.t1, vi, z.p1, z.p2), Block(a, z.t1, vi, z.p1, z.p2),
accelerate(0, z.t2, vmax, z.p2, z.p3), Block(0, z.t2, vmax, z.p2, z.p3),
accelerate(-a, z.t3, vmax, z.p3, z.p4), Block(-a, z.t3, vmax, z.p3, z.p4),
] ]
next_segment.entry_velocity = vexit next_segment.entry_velocity = vexit
i += 1 i += 1
continue else:
# accelerate, decelerate
# accelerate, decelerate? /\
if m.s1 > -EPS and m.s2 > -EPS:
segment.blocks = [ segment.blocks = [
accelerate(a, m.t1, vi, m.p1, m.p2), Block(a, m.t1, vi, m.p1, m.p2),
accelerate(-a, m.t2, m.vmax, m.p2, m.p3), Block(-a, m.t2, m.vmax, m.p2, m.p3),
] ]
next_segment.entry_velocity = vexit next_segment.entry_velocity = vexit
i += 1 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 # concatenate all of the blocks
blocks = [] blocks = []
@ -267,78 +225,3 @@ def constant_acceleration_plan(points, a, vmax, cf):
# filter out zero-duration blocks and return # filter out zero-duration blocks and return
blocks = [b for b in blocks if b.t > EPS] blocks = [b for b in blocks if b.t > EPS]
return Plan(blocks) 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

View File

@ -2,7 +2,7 @@ from axi import Planner
def main(): def main():
planner = Planner( 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) draws = list(PATHS)
jogs = [] jogs = []
for p1, p2 in zip(draws, draws[1:]): for p1, p2 in zip(draws, draws[1:]):
@ -13,9 +13,9 @@ def main():
print 'var PIECES = [' print 'var PIECES = ['
for i, path in enumerate(paths): for i, path in enumerate(paths):
print '[' print '['
plan = planner.jerk_plan(path) plan = planner.plan(path)
for b in plan.blocks: 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 '[%s],' % ','.join(map(str, record))
print '],' print '],'
print '];' print '];'

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

View File

@ -16,11 +16,11 @@ def main():
for r in range(20, 100, 20): for r in range(20, 100, 20):
points = circle(0, 0, r, 90) + points points = circle(0, 0, r, 90) + points
planner = Planner( planner = Planner(
acceleration=50, max_velocity=200, corner_factor=1, jerk=5000) acceleration=50, max_velocity=200, corner_factor=1)
plan = planner.plan(points) plan = planner.plan(points)
print 'var PIECES = [' print 'var PIECES = ['
for b in plan.blocks: 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 '[%s],' % ','.join(map(str, record))
print '];' print '];'