軌道(Trajectory)

$ maxima

Maxima 5.46.0 https://maxima.sourceforge.io
using Lisp GNU Common Lisp (GCL) GCL 2.6.14 git tag Version_2_6_15pre7
Distributed under the GNU Public License. See the file COPYING.
Dedicated to the memory of William Schelter.
The function bug_report() provides bug reporting information.

(%i1) x_ref: 2$

(%i2) v_ref: 1$

(%i3) [t0, t1]: [0, 1]$

(%i4) h: a*t^6 + b*t^5 + c*t^4 + d*t^3 + e*t^2 + f*t + g; /* velocity */
                     6      5      4      3      2
(%o4)             a t  + b t  + c t  + d t  + e t  + f t + g
(%i5) eq1: subst(t = t0, h) = 0/* v0 */;
(%o5)                                g = 0
(%i6) eq2: subst(t = (t0 + t1)/2, h) = v_ref/2;
                           f   e   d   c    b    a    1
(%o6)                  g + - + - + - + -- + -- + -- = -
                           2   4   8   16   32   64   2
(%i7) eq3: subst(t = t1, h) = v_ref;
(%o7)                    g + f + e + d + c + b + a = 1
(%i8) diff(h, t); /* acceleration */
                      5        4        3        2
(%o8)            6 a t  + 5 b t  + 4 c t  + 3 d t  + 2 e t + f
(%i9) eq4: subst(t = t0, %) = 0;
(%o9)                                f = 0
(%i10) eq5: subst(t = t1, %th(2)) = 0;
(%o10)                f + 2 e + 3 d + 4 c + 5 b + 6 a = 0
(%i11) diff(h, t, 2); /* jerk */
                         4         3         2
(%o11)             30 a t  + 20 b t  + 12 c t  + 6 d t + 2 e
(%i12) eq6: subst(t = t0, %) = 0;
(%o12)                              2 e = 0
(%i13) eq7: subst(t = t1, %th(2)) = 0;
(%o13)                2 e + 6 d + 12 c + 20 b + 30 a = 0
(%i14) solve([eq1, eq2, eq3, eq4, eq5, eq6, eq7], [a, b, c, d, e, f, g]);
(%o14)      [[a = 0, b = 6, c = - 15, d = 10, e = 0, f = 0, g = 0]]
(%i15) subst(%[1], h); /* velocity */
                                5       4       3
(%o15)                       6 t  - 15 t  + 10 t
(%i16) fortran(%);
      6*t**5-15*t**4+10*t**3
(%o16)                               done
(%i17) diff(%th(2), t); /* acceleration */
                                 4       3       2
(%o17)                       30 t  - 60 t  + 30 t
(%i18) fortran(%);
      30*t**4-60*t**3+30*t**2
(%o18)                               done
(%i19) diff(%th(4), t, 2); /* jerk */
                                 3        2
(%o19)                      120 t  - 180 t  + 60 t
(%i20) fortran(%);
      120*t**3-180*t**2+60*t
(%o20)                               done

(%i21) integrate(%th(6), t, t0, t1); /* position */
                                       1
(%o21)                                 -
                                       2
(%i22) if not((x_ref - 2*%) >= 0) then error("schedulig error!"); /* assert */
(%o22)                               false
(%i23) t2: t1 + (x_ref - 2*%th(2))/v_ref;
(%o23)                                 2
(%i24) t3: t2 + (t1 - t0);
(%o24)                                 3

(%i25) h; /* velocity */
                     6      5      4      3      2
(%o25)            a t  + b t  + c t  + d t  + e t  + f t + g
(%i26) eq1: subst(t = t2, %) = v_ref;
(%o26)           g + 2 f + 4 e + 8 d + 16 c + 32 b + 64 a = 1
(%i27) eq2: subst(t = (t2 + t3)/2, %th(2)) = v_ref/2;
                 5 f   25 e   125 d   625 c   3125 b   15625 a   1
(%o27)       g + --- + ---- + ----- + ----- + ------ + ------- = -
                  2     4       8      16       32       64      2
(%i28) eq3: subst(t = t3, %th(3)) = 0;
(%o28)          g + 3 f + 9 e + 27 d + 81 c + 243 b + 729 a = 0
(%i29) diff(h, t); /* acceleration */
                      5        4        3        2
(%o29)           6 a t  + 5 b t  + 4 c t  + 3 d t  + 2 e t + f
(%i30) eq4: subst(t = t2, %) = 0;
(%o30)             f + 4 e + 12 d + 32 c + 80 b + 192 a = 0
(%i31) eq5: subst(t = t3, %th(2)) = 0;
(%o31)            f + 6 e + 27 d + 108 c + 405 b + 1458 a = 0
(%i32) diff(h, t, 2); /* jerk */
                         4         3         2
(%o32)             30 a t  + 20 b t  + 12 c t  + 6 d t + 2 e
(%i33) eq6: subst(t = t2, %) = 0;
(%o33)               2 e + 12 d + 48 c + 160 b + 480 a = 0
(%i34) eq7: subst(t = t3, %th(2)) = 0;
(%o34)              2 e + 18 d + 108 c + 540 b + 2430 a = 0
(%i35) solve([eq1, eq2, eq3, eq4, eq5, eq6, eq7], [a, b, c, d, e, f, g]);
(%o35) [[a = 0, b = - 6, c = 75, d = - 370, e = 900, f = - 1080, g = 513]]
(%i36) subst(%[1], h); /* velocity */
                     5        4        3        2
(%o36)         (- 6 t ) + 75 t  - 370 t  + 900 t  - 1080 t + 513
(%i37) fortran(%);
      (-6*t**5)+75*t**4-370*t**3+900*t**2-1080*t+513
(%o37)                               done
(%i38) diff(%th(2), t); /* acceleration */
                        4         3         2
(%o38)           (- 30 t ) + 300 t  - 1110 t  + 1800 t - 1080
(%i39) fortran(%);
      (-30*t**4)+300*t**3-1110*t**2+1800*t-1080
(%o39)                               done
(%i40) diff(%th(4), t, 2); /* jerk */
                              3         2
(%o40)                (- 120 t ) + 900 t  - 2220 t + 1800
(%i41) fortran(%);
      (-120*t**3)+900*t**2-2220*t+1800
(%o41)                               done

x_ref = 2
v_ref = 1
t0, t1, t2, t3 = 0, 1, 2, 3

import numpy as np
import pandas as pd

dt = 0.001
data = pd.DataFrame({'t': np.arange(t0, t3 + dt, dt)})

data['v'] = data.t.apply(lambda t: 6*t**5-15*t**4+10*t**3 if t < t1 else ((-6*t**5)+75*t**4-370*t**3+900*t**2-1080*t+513 if t > t2 else v_ref)) # velocity
data['a'] = data.t.apply(lambda t: 30*t**4-60*t**3+30*t**2 if t < t1 else ((-30*t**4)+300*t**3-1110*t**2+1800*t-1080 if t > t2 else 0)) # accelerate
data['j'] = data.t.apply(lambda t: 120*t**3-180*t**2+60*t if t < t1 else ((-120*t**3)+900*t**2-2220*t+1800 if t > t2 else 0)) # jerk

if True: # Euler method
    if False: # adhoc
        data['dt'] = data.t.diff().fillna(0)
        data['dx'] = data.v*data.dt
    else:
        data['dx'] = data.v*dt
    data['x'] = data.dx.cumsum() # position
    if False: # check final position
        print(x_ref - data.x.iloc[-1])

import matplotlib
matplotlib.use('Agg')
import matplotlib.pyplot as plt

fig, axes = plt.subplots(4, 1)

for ax in axes:
    ax.grid()

ax = axes[0] # position
ax.set_xticks([t0, t1, t2, t3])
ax.set_xticklabels([])
ax.set_yticks([0, x_ref])
ax.set_yticklabels([0, r'$ x_\mathrm{ref}  $'])
ax.plot(data.t, data.x, ls='--', color='k', label='trajectory')
ax.legend(loc='best')
ax.set_ylabel(r'$ x,\, \theta $')

ax = axes[1] # velocity
ax.set_xticks([t0, (t0 + t1)/2, t1, t2, (t2 + t3)/2, t3])
ax.set_xticklabels([])
ax.set_yticks([0, v_ref/2, v_ref])
ax.set_yticklabels([0, None, r'$ v_\mathrm{ref} $'])
window = data[data.t < t1]
ax.plot(window.t, window.v, alpha=0.8, label=r'$ 6t^5 - 15t^4 + 10t^3 $')
window = data[(t1 <= data.t) & (data.t < t2)]
ax.plot(window.t, window.v, alpha=0.8)
window = data[t2 <= data.t]
ax.plot(window.t, window.v, alpha=0.8)
ax.legend(loc='best')
ax.set_ylabel(r'$ \dot x,\, \omega $')

ax = axes[2] # accelerate
ax.set_xticks([t0, t1, t2, t3])
ax.set_xticklabels([])
ax.set_yticks([data.a.min(), 0, data.a.max()])
ax.set_yticklabels([r'$ a_\min $', 0, r'$ a_\max $'])
window = data[data.t < t1]
ax.plot(window.t, window.a, alpha=0.8, label=r'$ 30t^4 - 60t^3 + 30t^2 $')
window = data[(t1 <= data.t) & (data.t < t2)]
ax.plot(window.t, window.a, alpha=0.8)
window = data[t2 <= data.t]
ax.plot(window.t, window.a, alpha=0.8)
ax.legend(loc='best')
ax.set_ylabel(r'$ \ddot x \propto F,\, \dot\omega \propto \tau $')

ax = axes[3] # jerk
ax.set_xticks([t0, t1, t2, t3])
ax.set_xticklabels([r'$ t_0 $', r'$ t_1 $', r'$ t_2 $', r'$ t_3 $'])
ax.set_yticks([0])
window = data[data.t < t1]
ax.plot(window.t, window.j, alpha=0.8, label=r'$ 120t^3 - 180t^2 + 60t $')
window = data[(t1 <= data.t) & (data.t < t2)]
ax.plot(window.t, window.j, alpha=0.8)
window = data[t2 <= data.t]
ax.plot(window.t, window.j, alpha=0.8)
ax.legend(loc='best')
ax.set_ylabel(r'$ \dddot x,\, \ddot\omega $')

axes[0].set_title('easing: smootherstep')
axes[3].set_xlabel('time')

fig.savefig('fig.svg')