diff --git a/.gitignore b/.gitignore
new file mode 100644
index 0000000000000000000000000000000000000000..bee8a64b79a99590d5303307144172cfe824fbf7
--- /dev/null
+++ b/.gitignore
@@ -0,0 +1 @@
+__pycache__
diff --git a/curves.py b/curves.py
new file mode 100644
index 0000000000000000000000000000000000000000..4a3aadc0351996ed48ead5c08ea7a90de3434c63
--- /dev/null
+++ b/curves.py
@@ -0,0 +1,120 @@
+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
diff --git a/curves/p.npy b/curves/p.npy
new file mode 100644
index 0000000000000000000000000000000000000000..8b34498dcf3181666b25fe14d6d3900ba5b1ad8b
Binary files /dev/null and b/curves/p.npy differ
diff --git a/curves/p_prime.npy b/curves/p_prime.npy
new file mode 100644
index 0000000000000000000000000000000000000000..a00dc31931dedb2d125dcff6fca6ca5b0728540b
Binary files /dev/null and b/curves/p_prime.npy differ
diff --git a/curves/p_prime2.npy b/curves/p_prime2.npy
new file mode 100644
index 0000000000000000000000000000000000000000..2ba851498aa33eaaa1b90fcb4cc53d2fcf424adc
Binary files /dev/null and b/curves/p_prime2.npy differ
diff --git a/curves/u.npy b/curves/u.npy
new file mode 100644
index 0000000000000000000000000000000000000000..db5f73d34a33111edf97ffafa41855a68631221a
Binary files /dev/null and b/curves/u.npy differ
diff --git a/curves/u_dot.npy b/curves/u_dot.npy
new file mode 100644
index 0000000000000000000000000000000000000000..8ddef6c76b92842f1593aa6ed25d31cbb35924e2
Binary files /dev/null and b/curves/u_dot.npy differ
diff --git a/plot_solution_video.py b/plot_solution_video.py
new file mode 100644
index 0000000000000000000000000000000000000000..cfa785dc8ea2ea20a66448d256af1a3684a7036d
--- /dev/null
+++ b/plot_solution_video.py
@@ -0,0 +1,117 @@
+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()
diff --git a/main_animate.py b/solve_animate.py
similarity index 100%
rename from main_animate.py
rename to solve_animate.py
diff --git a/solve_curves.py b/solve_curves.py
new file mode 100644
index 0000000000000000000000000000000000000000..021fcbd9a0aa9327659621a3bca610079f9043e2
--- /dev/null
+++ b/solve_curves.py
@@ -0,0 +1,211 @@
+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()