ND curve solver

.gitignore

+__pycache__

curves.py

+import numpy as np
+import math
+
+
+class Curve:
+    def __call__(self, u):
+        raise NotImplementedError()
+
+    def derivative(self, u):
+        raise NotImplementedError()
+
+    def second_derivative(self, u):
+        raise NotImplementedError()
+
+
+class Line1D(Curve):
+    def __init__(self, a, b):
+        self.a = a
+        self.b = b
+
+    def __call__(self, u):
+        v = 1 - u
+        return (self.a * v + self.b * u)[:, np.newaxis]
+
+    def derivative(self, u):
+        return u[:, np.newaxis]*0.0 + (self.b - self.a)
+
+    def second_derivative(self, u):
+        return u[:, np.newaxis]*0.0
+
+
+class Line2D(Curve):
+    def __init__(self, a, b):
+        self.a = a
+        self.b = b
+
+    def __call__(self, u):
+        v = 1 - u
+        n = len(u)
+        p = np.zeros((n, 2))
+        p[:, 0] = (self.a[0] * v + self.b[0] * u)
+        p[:, 1] = (self.a[1] * v + self.b[1] * u)
+        return p
+
+    def derivative(self, u):
+        n = len(u)
+        p = np.zeros((n, 2))
+        p[:, 0] = (self.b[0] - self.a[0])
+        p[:, 1] = (self.b[1] - self.a[1])
+        return p
+
+    def second_derivative(self, u):
+        n = len(u)
+        p = np.zeros((n, 2))
+        return p
+
+
+class Circle2D(Curve):
+    def __init__(self, r):
+        self.r = r
+
+    def __call__(self, u):
+        n = len(u)
+        p = np.zeros((n, 2))
+        p[:, 0] = np.cos(2*math.pi*u)
+        p[:, 1] = np.sin(2*math.pi*u)
+        return p
+
+    def derivative(self, u):
+        n = len(u)
+        p = np.zeros((n, 2))
+        p[:, 0] = -2*math.pi*np.sin(2*math.pi*u)
+        p[:, 1] = 2*math.pi*np.cos(2*math.pi*u)
+        return p
+
+    def second_derivative(self, u):
+        n = len(u)
+        p = np.zeros((n, 2))
+        p[:, 0] = -4*math.pi*math.pi*np.cos(2*math.pi*u)
+        p[:, 1] = -4*math.pi*math.pi*np.sin(2*math.pi*u)
+        return p
+
+
+class CubicBezier(Curve):
+    def __init__(self, A, B, C, D):
+        self.n_dims = len(A)
+        self.A = A
+        self.B = B
+        self.C = C
+        self.D = D
+
+    def __call__(self, u):
+        v = 1-u
+        n = len(u)
+        p = np.zeros((n, self.n_dims))
+        for i in range(self.n_dims):
+            p[:, i] += (v*v*v)*self.A[i]
+            p[:, i] += 3*v*v*u*self.B[i]
+            p[:, i] += 3*v*u*u*self.C[i]
+            p[:, i] += u*u*u*self.D[i]
+        return p
+
+    def derivative(self, u):
+        v = 1-u
+        n = len(u)
+        p = np.zeros((n, self.n_dims))
+        for i in range(self.n_dims):
+            p[:, i] += 3*v*v*(self.B[i]-self.A[i])
+            p[:, i] += 6*v*u*(self.C[i]-self.B[i])
+            p[:, i] += 3*u*u*(self.D[i]-self.C[i])
+        return p
+
+    def second_derivative(self, u):
+        v = 1 - u
+        n = len(u)
+        p = np.zeros((n, self.n_dims))
+        for i in range(self.n_dims):
+            p[:, i] += 6*v*(self.C[i]-2*self.B[i]+self.A[i])
+            p[:, i] += 6*u*(self.D[i]-2*self.C[i]+self.B[i])
+        return p

plot_solution_video.py

+import math
+import numpy as np
+import matplotlib.pyplot as plt
+from matplotlib.animation import FuncAnimation
+import time
+
+ani = None
+interval_ms = 16
+t = 0
+
+
+def update(frame, lines, u, u_dot, p, p_prime, p_prime2):
+    global ani, interval_ms, t
+
+    n = len(u)
+    u_dot_mid = (u_dot[:-1] + u_dot[1:])*0.5
+    t_delta = (u[1:] - u[:-1])/u_dot_mid
+    t_delta_ext = np.zeros((n,))
+    t_delta_ext[1:] = t_delta
+    t_axis = np.cumsum(t_delta_ext)
+    a = np.zeros((n-1, 2))
+    p_prime_mid = 0.5 * (p_prime[:-1, :] + p_prime[1:, :])
+    p_prime2_mid = 0.5 * (p_prime2[:-1, :] + p_prime2[1:, :])
+    u_dot_prime = (u_dot[1:] - u_dot[:-1]) / (u[1:] - u[:-1])
+    for i in range(2):
+        a[:, i] += p_prime2_mid[:, i] * u_dot_mid * u_dot_mid
+        a[:, i] += p_prime_mid[:, i] * u_dot_mid * u_dot_prime
+
+    if t > t_axis[-1]:
+        # cycle
+        t = 0
+        # time.sleep(0.5)
+
+    p_now = np.zeros((2,))
+    a_now = np.zeros((2,))
+    p_now[0] = np.interp(t, t_axis, p[:, 0])
+    p_now[1] = np.interp(t, t_axis, p[:, 1])
+    a_now[0] = np.interp(t, t_axis[:-1], a[:, 0])
+    a_now[1] = np.interp(t, t_axis[:-1], a[:, 1])
+
+    a_scale = 0.03
+
+    lines[0].set_xdata(p[:, 0])
+    lines[0].set_ydata(p[:, 1])
+    lines[1].set_xdata(p_now[0])
+    lines[1].set_ydata(p_now[1])
+    lines[2].set_xdata([p_now[0], p_now[0]+a_scale*a_now[0]])
+    lines[2].set_ydata([p_now[1], p_now[1]+a_scale*a_now[1]])
+
+    lines[4].set_xdata(t_axis)
+    lines[4].set_ydata(u_dot)
+    lines[5].set_xdata([t, t])
+    lines[5].set_ydata([0, np.max(u_dot)*1.5])
+
+    t += interval_ms/1000.0
+
+    return lines
+
+
+def main_func():
+    global ani, interval_ms
+
+    u = np.load("curves/u.npy")
+    u_dot = np.load("curves/u_dot.npy")
+    p = np.load("curves/p.npy")
+    p_prime = np.load("curves/p_prime.npy")
+    p_prime2 = np.load("curves/p_prime2.npy")
+    n = len(u)
+
+    u_mid = (u[:-1] + u[1:]) * 0.5
+    u_delta = u[1:] - u[:-1]
+    u_dot_mid = (u_dot[:-1] + u_dot[1:]) * 0.5
+
+    t_delta = u_delta / u_mid
+    a = (u_dot[1:] - u_dot[:-1]) / t_delta
+
+    t_delta_ext = np.zeros((n,))
+    t_delta_ext[1:] = t_delta
+    t = np.cumsum(t_delta_ext)
+
+    fig, axes = plt.subplots(1, 2)
+    ax = axes[0]
+    ax2 = axes[1]
+    ln0, = ax.plot([], [], 'b')
+    ln1, = ax.plot([], [], 'k.', ms=20)
+    ln2, = ax.plot([], [], 'r-')
+    ln3, = ax.plot([], [], 'g-')
+    ax.set_xlim([np.min(p[:, 0])-0.5, np.max(p[:, 0])+0.5])
+    ax.set_ylim([np.min(p[:, 1])-0.5, np.max(p[:, 1])+0.5])
+    ax.set_aspect("equal")
+    ax.set_xlabel("x")
+    ax.set_ylabel("y")
+    ax.set_title("Motion")
+    # ax.legend(["speed", "min", "max"])
+
+    ln4, = ax2.plot([], [], 'b')
+    ln5, = ax2.plot([], [], 'k')
+    ln6, = ax2.plot([], [], 'r')
+    ax2.set_xlim([0, 2])
+    ax2.set_ylim([0, 1])
+    ax2.set_xlabel("t")
+    ax2.set_ylabel("u_dot")
+    ax2.set_title("Curve reading speed")
+    # ax2.legend(["acc.", "min", "max"])
+
+    lines = [ln0, ln1, ln2, ln3, ln4, ln5, ln6]
+    plt.tight_layout()
+
+    def init():
+        return lines
+
+    ani = FuncAnimation(fig, update, init_func=init, fargs=(lines, u, u_dot, p, p_prime, p_prime2), blit=True, interval=interval_ms)
+    plt.show()
+
+
+if __name__ == "__main__":
+    main_func()

solve_curves.py

+import numpy as np
+import matplotlib.pyplot as plt
+import curves
+
+
+# def a_mp_func(v, v1=0.2, a0=7.0, ap=5.0, am=7.0):
+#     n = len(v)
+#     a_minus = np.zeros((n,))
+#     a_plus = np.zeros((n,))
+#     alpha1 = np.minimum(v[v > 0]/v1, 1)
+#     alpha2 = np.minimum(-v[v <= 0]/v1, 1)
+#     a_minus[v > 0] = -(alpha1 * am + (1-alpha1) * a0)
+#     a_plus[v > 0] = alpha1 * ap + (1-alpha1) * a0
+#     a_minus[v <= 0] = -(alpha2 * ap + (1-alpha2) * a0)
+#     a_plus[v <= 0] = alpha2 * am + (1-alpha2) * a0
+#     return a_minus, a_plus
+
+
+def a_mp_func(v, ap=5.0):
+    # basic, constant acceleration constraint
+    n = len(v)
+    a_minus = np.zeros((n,))
+    a_plus = np.zeros((n,))
+    a_plus[:] = ap
+    a_minus[:] = -ap
+    return a_minus, a_plus
+
+
+def constraints_a(p_prime, p_prime2, u, u_dot, jerk_max=50.0, show=False):
+    n, n_dims = p_prime.shape
+
+    a = np.zeros((n-1, n_dims))
+
+    a_minus = np.zeros((n-1, n_dims))
+    a_plus = np.zeros((n-1, n_dims))
+
+    u_mid = 0.5 * (u[:-1] + u[1:])
+    u_dot_mid = 0.5 * (u_dot[:-1] + u_dot[1:])
+    p_prime_mid = 0.5 * (p_prime[:-1, :] + p_prime[1:, :])
+    p_prime2_mid = 0.5 * (p_prime2[:-1, :] + p_prime2[1:, :])
+    u_dot_prime = (u_dot[1:] - u_dot[:-1]) / (u[1:] - u[:-1])
+    dt = (u[1:] - u[:-1]) / u_dot_mid
+    dt_a = (dt[:-1] + dt[1:]) * 0.5
+
+    u_dot2_min_all = -np.inf * np.ones((n - 1, n_dims))
+    u_dot2_max_all = np.inf * np.ones((n - 1, n_dims))
+
+    v = np.zeros((n-1, n_dims))
+    for i in range(n_dims):
+        v[:, i] = p_prime_mid[:, i] * u_dot_mid
+        a[:, i] += p_prime2_mid[:, i] * u_dot_mid * u_dot_mid
+        a[:, i] += p_prime_mid[:, i] * u_dot_mid * u_dot_prime
+
+        a_plus_next = a[:-1, i] + jerk_max * dt_a
+        a_minus_next = a[:-1, i] - jerk_max * dt_a
+        a_plus_prev = a[1:, i] + jerk_max * dt_a
+        a_minus_prev = a[1:, i] - jerk_max * dt_a
+
+        # boundary condition
+        # a_minus[0, i] = a_minus_prev[0]
+        # a_minus[-1, i] = a_minus_next[-1]
+        # a_plus[0, i] = a_plus_prev[0]
+        # a_plus[-1, i] = a_plus_next[-1]
+
+        # zero acceleration beyond curve
+        a_minus[0] = -jerk_max * dt_a[0]
+        a_plus[0] = jerk_max * dt_a[0]
+        a_minus[-1] = -jerk_max * dt_a[-1]
+        a_plus[-1] = jerk_max * dt_a[-1]
+
+        a_minus[1:-1, i] = np.maximum(a_minus_next[:-1], a_minus_prev[1:])
+        a_plus[1:-1, i] = np.minimum(a_plus_next[:-1], a_plus_prev[1:])
+
+        # min and max prescribed by speed
+        a_neg_bound, a_pos_bound = a_mp_func(v[:, i])
+        a_minus[:, i] = np.minimum(a_minus[:, i], a_pos_bound)
+        a_minus[:, i] = np.maximum(a_minus[:, i], a_neg_bound)
+        a_plus[:, i] = np.minimum(a_plus[:, i], a_pos_bound)
+        a_plus[:, i] = np.maximum(a_plus[:, i], a_neg_bound)
+
+        # acceleration along the curve only
+        a_plus_along = a_plus[:, i] - p_prime2_mid[:, i] * u_dot_mid*u_dot_mid
+        a_minus_along = a_minus[:, i] - p_prime2_mid[:, i] * u_dot_mid*u_dot_mid
+
+        mask = np.abs(p_prime_mid[:, i]) > 1e-7
+
+        u_dot2_1 = a_plus_along[mask]/p_prime_mid[mask, i]
+        u_dot2_2 = a_minus_along[mask]/p_prime_mid[mask, i]
+        u_dot2_min_all[mask, i] = np.minimum(u_dot2_1, u_dot2_2)
+        u_dot2_max_all[mask, i] = np.maximum(u_dot2_1, u_dot2_2)
+
+    u_dot2_min = np.max(u_dot2_min_all, axis=-1)
+    u_dot2_max = np.min(u_dot2_max_all, axis=-1)
+
+    if show:
+        plt.figure()
+        plt.plot(u_mid, u_dot_prime * u_dot_mid, 'k')
+        plt.plot(u_mid, u_dot2_min, 'b')
+        plt.plot(u_mid, u_dot2_max, 'r')
+        plt.title("u_dot2")
+
+        plt.figure()
+        for i in range(n_dims):
+            plt.subplot(n_dims, 1, i+1)
+            plt.plot(u_mid, u_dot2_min_all[:, i], 'b')
+            plt.plot(u_mid, u_dot2_max_all[:, i], 'r')
+            plt.title(f"u_dot2_{i}")
+
+        plt.figure()
+        for i in range(n_dims):
+            plt.subplot(n_dims, 1, i+1)
+            plt.plot(u_mid, a[:, i], 'k')
+            plt.plot(u_mid, a_minus[:, i], 'b')
+            plt.plot(u_mid, a_plus[:, i], 'r')
+            plt.title(f"a_{i}")
+
+    return u_dot2_min, u_dot2_max
+
+
+def constraints_v(p_prime, u, u_dot, u_dot2_min, u_dot2_max, v_abs_max=4.0, show=False):
+    n, n_dims = p_prime.shape
+    u_dot_min = np.zeros((n,))
+    u_dot_max = np.zeros((n,))
+
+    # v = np.zeros((n - 1, n_dims))
+    du = u[1:] - u[:-1]
+    u_dot_max_next = np.sqrt(np.maximum(u_dot[:-1]**2 + 2 * u_dot2_max * du, 0))
+    u_dot_min_next = np.sqrt(np.maximum(u_dot[:-1]**2 + 2 * u_dot2_min * du, 0))
+    u_dot_min_prev = np.sqrt(np.maximum(u_dot[1:]**2 - 2 * u_dot2_max * du, 0))
+    u_dot_max_prev = np.sqrt(np.maximum(u_dot[1:]**2 - 2 * u_dot2_min * du, 0))
+    u_dot_min[0] = u_dot_min_prev[0]
+    u_dot_min[-1] = u_dot_min_next[-1]
+    u_dot_max[0] = u_dot_max_prev[0]
+    u_dot_max[-1] = u_dot_max_next[-1]
+    u_dot_min[1:-1] = np.maximum(u_dot_min_next[:-1], u_dot_min_prev[1:])
+    u_dot_max[1:-1] = np.minimum(u_dot_max_next[:-1], u_dot_max_prev[1:])
+    u_dot_min = np.minimum(u_dot_min, u_dot_max)
+
+    p_prime_norm = np.linalg.norm(p_prime, axis=-1)
+    u_dot_min[:] = np.maximum(u_dot_min, 0)
+    u_dot_max[:] = np.minimum(u_dot_max, v_abs_max / p_prime_norm)
+
+    if show:
+        plt.figure()
+        plt.plot(u, u_dot, 'k')
+        plt.plot(u, u_dot_min, 'b')
+        plt.plot(u, u_dot_max, 'r')
+        plt.title("u_dot")
+
+    return u_dot_min, u_dot_max
+
+
+def main():
+    n = 256
+    # c = curves.Line1D(0, 4)
+    # c = curves.Line2D([0, 0], [4, 0])
+    # c = curves.CubicBezier([0, 0], [1, 0], [1, 2], [0, 0.7])
+    c = curves.CubicBezier([0, 0], [1, 0], [1, 1], [2, 1])
+    # c = curves.Circle2D(1.5)
+    u = np.linspace(0, 1, n)
+    u_dot = np.zeros((n,))
+    p = c(u)
+    p_prime = c.derivative(u)
+    p_prime2 = c.second_derivative(u)
+
+    u_dot[1:-1] = 1e-4
+    # u_dot[:] = 0.1*(2*u - 2*u*u)
+
+    # lambd = 0.1
+    lambd = 0.3
+    optimizing = True
+    max_itr = int(5e4)
+    k = 0
+    while optimizing:
+        show = k == -1
+        u_dot2_min, u_dot2_max = constraints_a(p_prime, p_prime2, u, u_dot, show=show)
+        u_dot_min, u_dot_max = constraints_v(p_prime, u, u_dot, u_dot2_min, u_dot2_max, show=show)
+
+        if show:
+            plt.show()
+            return
+
+        mse_diff = np.mean(np.square(u_dot[1:-1] - u_dot_max[1:-1]))
+        u_dot[1:-1] = lambd * u_dot_max[1:-1] + (1 - lambd) * u_dot[1:-1]
+        k += 1
+        optimizing = (mse_diff > 1e-10) and (k < max_itr)
+
+    np.save("curves/u.npy", u)
+    np.save("curves/u_dot.npy", u_dot)
+    np.save("curves/p.npy", p)
+    np.save("curves/p_prime.npy", p_prime)
+    np.save("curves/p_prime2.npy", p_prime2)
+
+    print(f"{k} steps")
+
+    u_mid = (u[:-1] + u[1:]) * 0.5
+
+    plt.figure()
+    plt.plot(u, u_dot, 'k')
+    plt.plot(u, u_dot_min, 'b')
+    plt.plot(u, u_dot_max, 'r')
+
+    plt.figure()
+    plt.plot(u_mid, u_dot2_min, 'b')
+    plt.plot(u_mid, u_dot2_max, 'r')
+
+    plt.show()
+
+
+if __name__ == "__main__":
+    main()