Skip to content

Instantly share code, notes, and snippets.

@mincrmatt12
Last active September 14, 2019 18:26
Show Gist options
  • Save mincrmatt12/2ae3e03182d529d933bf4caf0a78f001 to your computer and use it in GitHub Desktop.
Save mincrmatt12/2ae3e03182d529d933bf4caf0a78f001 to your computer and use it in GitHub Desktop.
fit thingy
import math
"""
routines:
- fit spline (compute x_dot and x_dot_dot)
- interpolate_with_spline (compute full range of x, x_dot, x_dot_dot from spline output for visualization)
- init_times (compute times from velocity mins/maxs) max accel will limit this further
"""
def fit_spline(x_values, x_times, initial_velocity=0, final_velocity=0):
"""
fit a cubic spline to the set of values in x_values (x representing state here) with ACTUAL x-values (time here) in x_times.
initial_velocity and final_velocity control the set values to the first derivates at both ends
"""
n = len(x_values)
dt = [x_times[j] - x_times[j-1] for j in range(1, n)]
# transcribed from the moveit source code
c = [0 for x in range(n)] # velocities
d = [0 for x in range(n)] # accelerations
c[0] = 0.5
d[0] = 3.0 * ((x_values[1] - x_values[0]) / dt[0] - initial_velocity) / dt[0];
for i in range(1, n - 1):
dt2 = dt[i - 1] + dt[i]
a = dt[i - 1] / dt2
denom = 2.0 - a * c[i - 1]
c[i] = (1.0 - a) / denom
d[i] = 6.0 * ((x_values[i + 1] - x_values[i]) / dt[i] - (x_values[i] - x_values[i - 1]) / dt[i - 1]) / dt2
d[i] = (d[i] - a * d[i -1]) / denom
denom = dt[n - 2] * (2.0 - c[n - 2])
d[n - 1] = 6.0 * (final_velocity - (x_values[n - 1] - x_values[n - 2]) / dt[n - 2])
d[n - 1] = (d[n - 1] - dt[n - 2] * d[n - 2]) / denom
# backsubstitute correctly?
accelerations = [0 for x in range(n)]
velocities = [0 for x in range(n)]
accelerations[-1] = d[-1]
for i in range(n - 2, -1, -1):
accelerations[i] = d[i] - c[i] * accelerations[i + 1]
velocities[0] = initial_velocity
for i in range(1, n-1):
velocities[i] = (x_values[i + 1] - x_values[i]) / dt[i] - (2 * accelerations[i] + accelerations[i + 1]) * dt[i] / 6.0
velocities[n - 1] = final_velocity
return velocities, accelerations
def interpolate_with_spline(positions, velocities, accelerations, times, precision=0.02):
"""
computes values of of pos, vel and acc at all times with precision 0.02
does it by integrating:
V during a spline = ((jrk*t^2)/2 + p_acc*t) + previous V
P during a spline = ((jrk*t^3)/6 + (p_acc*t^2)/2 + previous v) + previous P
where jrk is (accel1 - accel2) / dt and p_acc is the initial acceleration
"""
pos_rec = []
vel_rec = []
acc_rec = []
jrk_rec = []
t = 0
idx_0 = 0
idx_1 = 1
while t < times[-1]:
if t > times[idx_1]:
idx_0 += 1
idx_1 += 1
p_acc = accelerations[idx_0]
jrk = (accelerations[idx_1] - accelerations[idx_0]) / (times[idx_1] - times[idx_0])
p_vel = velocities[idx_0]
p_pos = positions[idx_0]
t_ = t - times[idx_0]
jrk_rec.append(jrk)
acc_rec.append(jrk * t_ + p_acc)
vel_rec.append((jrk * t_ ** 2)/2 + p_acc * t_ + p_vel)
pos_rec.append((jrk * t_ ** 3)/6 + (p_acc * t_ ** 2)/2 + p_vel * t_ + p_pos)
t += precision
return pos_rec, vel_rec, acc_rec, jrk_rec
def init_times(positions, max_vel):
"""
compute the minimum required time to get from point a to point b for each point (using the max velocity)
effectively dx / max_vel with correct sign for the entire array
"""
print(positions)
times = [0 for x in range(len(positions[0]))]
t = 0
for i in range(1, len(positions[0])):
dx = 0
for j in positions:
dx = max(dx, abs(j[i] - j[i - 1]))
print(dx, i, j, len(times))
t += (dx / max_vel) + 0.000001
times[i] = t
return times
def adjust_boundary_acceleration(positions, times, init_acc, final_acc, init_vel, final_vel):
positions[1] = positions[0]
positions[-2] = positions[-3]
_, acc = fit_spline(positions, times, init_vel, final_vel)
a0 = acc[0]
b0 = acc[-1]
positions[1] = positions[2]
positions[-2] = positions[-1]
_, acc = fit_spline(positions, times, init_vel, final_vel)
a2 = acc[0]
b2 = acc[-1]
if a2 != a0:
positions[1] = positions[0] + ((positions[2] - positions[0]) / (a2 - a0)) * (init_acc - a0)
if b2 != b0:
positions[-2] = positions[-3] + ((positions[-1] - positions[-3]) / (b2 - b0)) * (final_acc - b0)
def fit_and_adjust(positions, max_vel, max_acc, max_jrk, init_vel=0, init_acc=0, final_vel=0, final_acc=0):
"""
time-parameterize the given path.
the path is specified as a nested list of positions. each nested array corresponds to a degree of freedom.
"""
positions = positions[:]
print(positions)
# step 1: check if we need to add extra points
if init_acc is not None:
for local_pos in positions:
local_pos.insert(1, (local_pos[0] * 9 + local_pos[1]) / 10)
local_pos.insert(len(local_pos)-1, (local_pos[-1] * 9 + local_pos[-2]) / 10)
times = init_times(positions, max_vel)
print(times)
while True:
is_done = True
scale_factors = [1 for x in range(len(positions[0])-1)]
if init_acc is not None:
for j in positions:
print(j, times)
adjust_boundary_acceleration(j, times, init_acc, final_acc, init_vel, final_vel)
for local_pos in positions:
velocities, accelerations = fit_spline(local_pos, times, init_vel, final_vel)
for i, acc in enumerate(accelerations):
atfactor = 1
if abs(acc) > max_acc:
atfactor = math.sqrt(acc / math.copysign(max_acc, acc))
if atfactor > 1.005:
is_done = False
atfactor = (atfactor - 1.0) / (len(positions) * 8.0) + 1.0
if i > 0:
scale_factors[i - 1] = max(scale_factors[i - 1], atfactor)
if i < len(positions[0]) - 1:
scale_factors[i] = max(scale_factors[i], atfactor)
if is_done:
break
dt = [times[j] - times[j-1] for j in range(1, len(positions[0]))]
dt = [x * y for x, y in zip(dt, scale_factors)]
t = 0
times = [t]
for v in dt:
t += v
times.append(t)
# attempt to correct for jerk
if abs(max_jrk) > 0.0001:
while True:
deltas = [1 for x in range(len(positions[0])-1)]
go = False
for i in range(len(positions[0])-1):
dt = times[i + 1] - times[i]
dt_fac = 1
for local_pos in positions:
_, accelerations = fit_spline(local_pos, times, init_vel, final_vel)
jrk = (accelerations[i + 1] - accelerations[i]) / dt
if abs(jrk) > max_jrk:
dt_fac = max(dt_fac, jrk / math.copysign(max_jrk, jrk))
go = True
print(dt_fac)
deltas[i] = dt * dt_fac
if not go:
break
t = 0
times = [t]
for v in deltas:
t += v
times.append(t)
# after jerk was corrected we need to iron out all the errors in acceleration (which at this point should be within 1%)
# final adjustment of boundary conditions
if init_acc is not None:
for j in positions:
adjust_boundary_acceleration(j, times, init_acc, final_acc, init_vel, final_vel)
velocities, accelerations = [], []
for j in positions:
v_l, a_l = fit_spline(j, times, init_vel, final_vel)
velocities.append(v_l)
accelerations.append(a_l)
return positions, velocities, accelerations, times
import matplotlib.pyplot as plt
import numpy as np
from fit import fit_and_adjust, interpolate_with_spline
step = 0.1
def goto(x, a):
if x < a:
return list(np.arange(x, a, step))
else:
return list(np.arange(x, a, -step))
def stay(x, d):
return [x for r in range(len(goto(x, x-d)))]
values = [goto(0, 2) + [2]]
vel_m = 1
acc_m = 2.5
jrk_m = 50
a_positions, a_velocities, a_accelerations, times = fit_and_adjust(values, vel_m, acc_m, jrk_m, 0)
precision = times[-1] / 10000.0
fig, ax = plt.subplots(2, 2, sharex=True)
ax[0, 0].set_ylabel("position")
ax[0, 1].set_ylabel("velocity")
ax[1, 0].set_ylabel("acceleration")
ax[1, 1].set_ylabel("jerk")
ax[1, 0].set_xlabel("time")
ax[1, 1].set_xlabel("time")
ax[0, 1].hlines([-vel_m, vel_m], times[0], times[-1], colors='g', linestyles='dashed')
ax[1, 0].hlines([-acc_m, acc_m], times[0], times[-1], colors='g', linestyles='dashed')
ax[1, 1].hlines([-jrk_m, jrk_m], times[0], times[-1], colors='g', linestyles='dashed')
for j, positions, velocities, accelerations in zip(range(len(a_positions)), a_positions, a_velocities, a_accelerations):
p_interp, v_interp, a_interp, j_interp = interpolate_with_spline(positions, velocities, accelerations, times, precision)
t_interp = np.arange(0, times[-1], precision)
if len(t_interp) > len(p_interp):
t_interp = t_interp[:-1]
elif len(t_interp) < len(p_interp):
p_interp = p_interp[:-1]
v_interp = v_interp[:-1]
a_interp = a_interp[:-1]
j_interp = j_interp[:-1]
ax[0, 0].plot(t_interp, p_interp, c='bgyrcmkw'[j])
ax[0, 0].scatter(times, positions, c='rgbywmkc'[j])
ax[0, 1].plot(t_interp, v_interp, c='bgyrcmkw'[j])
ax[0, 1].scatter(times, velocities, c='rgbywmkc'[j])
ax[1, 0].plot(t_interp, a_interp, c='bgyrcmkw'[j])
ax[1, 0].scatter(times, accelerations, c='rgbywmkc'[j])
ax[1, 1].plot(t_interp, j_interp, c='bgyrcmkw'[j])
plt.show()
@mincrmatt12
Copy link
Author

todo:

  • multi dof
  • proper jerk limiting

Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment