python/test/3rd/trajpoly5.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 t = (0, 1)
  18 p = (0, 1)
  19 v = (0, 1)
  20 traj = liba.trajpoly5(t[1] - t[0], p[0], p[1], v[0], v[1])
  21 data = np.arange(t[0], t[1], (t[1] - t[0]) / 1000)
  22
  23 text_p = "p="
  24 if traj.p[0]:
  25     text_p += "%g" % (traj.p[0])
  26 if traj.p[1]:
  27     text_p += "%+g%s" % (traj.p[1], "t")
  28 if traj.p[2]:
  29     text_p += "%+g%s" % (traj.p[2], "t^2")
  30 if traj.p[3]:
  31     text_p += "%+g%s" % (traj.p[3], "t^3")
  32 if traj.p[4]:
  33     text_p += "%+g%s" % (traj.p[4], "t^4")
  34 if traj.p[5]:
  35     text_p += "%+g%s" % (traj.p[5], "t^5")
  36 text_p = text_p.replace("=+", "=")
  37
  38 text_v = "v="
  39 if traj.v[0]:
  40     text_v += "%g" % (traj.v[0])
  41 if traj.v[1]:
  42     text_v += "%+g%s" % (traj.v[1], "t")
  43 if traj.v[2]:
  44     text_v += "%+g%s" % (traj.v[2], "t^2")
  45 if traj.v[3]:
  46     text_v += "%+g%s" % (traj.v[3], "t^3")
  47 if traj.v[4]:
  48     text_v += "%+g%s" % (traj.v[4], "t^4")
  49 text_v = text_v.replace("=+", "=")
  50
  51 text_a = "a="
  52 if traj.a[0]:
  53     text_a += "%g" % (traj.a[0])
  54 if traj.a[1]:
  55     text_a += "%+g%s" % (traj.a[1], "t")
  56 if traj.a[2]:
  57     text_a += "%+g%s" % (traj.a[2], "t^2")
  58 if traj.a[3]:
  59     text_a += "%+g%s" % (traj.a[3], "t^3")
  60 text_a = text_a.replace("=+", "=")
  61
  62 plt.figure("5 polynomial trajectory")
  63
  64 plt.subplot(311)
  65 plt.title("puintic polynomial trajectory")
  66 plt.ylabel("Position")
  67 plt.plot(data, np.array(traj.pos(data), copy=False), "r-", label=text_p)
  68 plt.legend()
  69
  70 plt.subplot(312)
  71 plt.ylabel("Velocity")
  72 plt.plot(data, np.array(traj.vel(data), copy=False), "b-", label=text_v)
  73 plt.legend()
  74
  75 plt.subplot(313)
  76 plt.ylabel("Acceleration")
  77 plt.plot(data, np.array(traj.acc(data), copy=False), "g-", label=text_a)
  78 plt.legend()
  79
  80 plt.xlabel("t")
  81 plt.savefig(os.path.join(base, "trajectory_polynomial_5.png"))
  82 plt.show()