python/test/3rd/trajtrap.py

   1 #!/usr/bin/env python
   2 import os, sys
   3
   4 base = os.path.dirname(__file__)
   5 path = os.path.dirname(base)
   6 path = os.path.dirname(path)
   7 sys.path.insert(0, path)
   8 try:
   9     import numpy as np
  10     import matplotlib.pyplot as plt
  11 except Exception as e:
  12     print(e)
  13     exit()
  14
  15 import liba  # type: ignore
  16
  17 traj = liba.trajtrap()
  18 traj.gen(2, 2, -2, 0, 4)
  19 data = np.arange(0, traj.t, traj.t / 1000)
  20
  21 plt.figure("trapezoidal velocity trajectory")
  22
  23 plt.subplot(311)
  24 plt.title("trapezoidal velocity trajectory")
  25 plt.ylabel("Position")
  26 plt.plot(data, np.array(traj.pos(data), copy=False), "r-", label="q")
  27 plt.legend()
  28
  29 plt.subplot(312)
  30 plt.ylabel("Velocity")
  31 plt.plot(data, np.array(traj.vel(data), copy=False), "b-", label="v")
  32 plt.legend()
  33
  34 plt.subplot(313)
  35 plt.ylabel("Acceleration")
  36 plt.plot(data, np.array(traj.acc(data), copy=False), "g-", label="a")
  37 plt.legend()
  38
  39 plt.xlabel("t")
  40 plt.savefig(os.path.join(base, "trajectory_trapezoidal.png"))
  41 plt.show()