python/test/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 sys.path.insert(0, path)
   7 if len(sys.argv) > 1:
   8     sys.stdout = open(sys.argv[1], "w")
   9
  10 import liba  # type: ignore
  11
  12 t = (0, 1)
  13 p = (0, 1)
  14 v = (0, 1)
  15 traj = liba.trajpoly5(t[1] - t[0], p[0], p[1], v[0], v[1])
  16
  17 N = 1000
  18 x = [0.0] * N
  19 T = (t[1] - t[0]) / N
  20 for i in range(N):
  21     x[i] = i * T
  22
  23 p = traj.pos(x)
  24 v = traj.vel(x)
  25 a = traj.acc(x)
  26
  27 for i in range(N):
  28     print("%g,%g,%g,%g" % (x[i], p[i], v[i], a[i]))
  29
  30 traj.p
  31 traj.v
  32 traj.a