python/test/trajpoly7.py

   1 #!/usr/bin/env python
   2 import os, sys
   3
   4 sys.path.insert(0, os.getcwd())
   5 prefix = os.path.join(sys.path[0], "build")
   6 if not os.path.exists(prefix):
   7     os.mkdir(prefix)
   8 try:
   9     import liba as a
  10     import numpy as np
  11     import matplotlib.pyplot as plt
  12 except Exception as e:
  13     print(e)
  14     exit()
  15
  16 t = (0, 1)
  17 p = (0, 1)
  18 v = (0, 1)
  19 data = np.arange(t[0], t[1], 0.01)
  20 traj = a.trajpoly7(t[1] - t[0], p[0], p[1], v[0], v[1])
  21 text_p = "p="
  22 if traj.p[0]:
  23     text_p += "%g" % (traj.p[0])
  24 if traj.p[1]:
  25     text_p += "%+g%s" % (traj.p[1], "t")
  26 if traj.p[2]:
  27     text_p += "%+g%s" % (traj.p[2], "t^2")
  28 if traj.p[3]:
  29     text_p += "%+g%s" % (traj.p[3], "t^3")
  30 if traj.p[4]:
  31     text_p += "%+g%s" % (traj.p[4], "t^4")
  32 if traj.p[5]:
  33     text_p += "%+g%s" % (traj.p[5], "t^5")
  34 if traj.p[6]:
  35     text_p += "%+g%s" % (traj.p[6], "t^6")
  36 if traj.p[7]:
  37     text_p += "%+g%s" % (traj.p[7], "t^7")
  38 text_p = text_p.replace("=+", "=")
  39 text_v = "v="
  40 if traj.v[0]:
  41     text_v += "%g" % (traj.v[0])
  42 if traj.v[1]:
  43     text_v += "%+g%s" % (traj.v[1], "t")
  44 if traj.v[2]:
  45     text_v += "%+g%s" % (traj.v[2], "t^2")
  46 if traj.v[3]:
  47     text_v += "%+g%s" % (traj.v[3], "t^3")
  48 if traj.v[4]:
  49     text_v += "%+g%s" % (traj.v[4], "t^4")
  50 if traj.v[5]:
  51     text_v += "%+g%s" % (traj.v[5], "t^5")
  52 if traj.v[6]:
  53     text_v += "%+g%s" % (traj.v[6], "t^6")
  54 text_v = text_v.replace("=+", "=")
  55 text_a = "a="
  56 if traj.a[0]:
  57     text_a += "%g" % (traj.a[0])
  58 if traj.a[1]:
  59     text_a += "%+g%s" % (traj.a[1], "t")
  60 if traj.a[2]:
  61     text_a += "%+g%s" % (traj.a[2], "t^2")
  62 if traj.a[3]:
  63     text_a += "%+g%s" % (traj.a[3], "t^3")
  64 if traj.a[4]:
  65     text_a += "%+g%s" % (traj.a[4], "t^4")
  66 if traj.a[5]:
  67     text_a += "%+g%s" % (traj.a[5], "t^5")
  68 text_a = text_a.replace("=+", "=")
  69 text_j = "j="
  70 if traj.j[0]:
  71     text_j += "%g" % (traj.j[0])
  72 if traj.j[1]:
  73     text_j += "%+g%s" % (traj.j[1], "t")
  74 if traj.j[2]:
  75     text_j += "%+g%s" % (traj.j[2], "t^2")
  76 if traj.j[3]:
  77     text_j += "%+g%s" % (traj.j[3], "t^3")
  78 if traj.j[4]:
  79     text_j += "%+g%s" % (traj.j[4], "t^4")
  80 text_j = text_j.replace("=+", "=")
  81 plt.figure("7 polynomial trajectory")
  82
  83 plt.subplot(411)
  84 plt.title("hepta polynomial trajectory")
  85 plt.ylabel("Position")
  86 plt.plot(data, traj.pos(data), "r-", label=text_p)
  87 plt.legend()
  88
  89 plt.subplot(412)
  90 plt.ylabel("Velocity")
  91 plt.plot(data, traj.vel(data), "b-", label=text_v)
  92 plt.legend()
  93
  94 plt.subplot(413)
  95 plt.ylabel("Acceleration")
  96 plt.plot(data, traj.acc(data), "g-", label=text_a)
  97 plt.legend()
  98
  99 plt.subplot(414)
 100 plt.ylabel("Jerk")
 101 plt.plot(data, traj.jer(data), "k-", label=text_j)
 102 plt.legend()
 103
 104 plt.xlabel("t")
 105 plt.savefig(os.path.join(prefix, "trajectory_polynomial_7.png"))