fit thingy

fit.py

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

test.py

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

todo:

• multi dof
• proper jerk limiting