add plotting utility

.gitignore

@@ -0,0 +1,2 @@
+build/
+__pycache__/

README.md

@@ -36,3 +36,13 @@ Example:
 ```
 ./genTrajectory -i ../examples/waypoints1.csv --v_max 1.0 --a_max 1.0 -o traj1.csv
 ```
+
+### Visualize Trajectory
+
+A python script can be used to visualize a trajectory csv-file (3D plot, velocity, acceleration, angular velocity, yaw). The first argument is the file and the second argument is the quadcopter of the UAV in kg.
+
+Example:
+
+```
+python3 ../scripts/plot_trajectory.py traj1.csv 0.035
+```

scripts/plot_trajectory.py

@@ -0,0 +1,53 @@
+#!/usr/bin/env python
+
+import numpy as np
+import matplotlib.pyplot as plt
+from mpl_toolkits.mplot3d import Axes3D
+import matplotlib.gridspec as gridspec
+import argparse
+
+import uav_trajectory
+
+if __name__ == "__main__":
+  parser = argparse.ArgumentParser()
+  parser.add_argument("trajectory", type=str, help="CSV file containing trajectory")
+  parser.add_argument("mass", type=float, help="mass of UAV [kg]")
+  args = parser.parse_args()
+
+  traj = uav_trajectory.Trajectory()
+  traj.loadcsv(args.trajectory)
+
+  ts = np.arange(0, traj.duration, 0.01)
+  evals = np.empty((len(ts), 13))
+  for t, i in zip(ts, range(0, len(ts))):
+    e = traj.eval(args.mass, t)
+    evals[i, 0:3]  = e.pos
+    evals[i, 3:6]  = e.vel
+    evals[i, 6:9]  = e.acc
+    evals[i, 9:12] = e.omega
+    evals[i, 12]   = e.yaw
+
+  # Create 3x1 sub plots
+  gs = gridspec.GridSpec(6, 1)
+  fig = plt.figure()
+
+  ax = plt.subplot(gs[0:2, 0], projection='3d') # row 0
+  ax.plot(evals[:,0], evals[:,1], evals[:,2])
+
+  ax = plt.subplot(gs[2, 0]) # row 2
+  ax.plot(ts, np.linalg.norm(evals[:,3:6], axis=1))
+  ax.set_ylabel("velocity [m/s]")
+
+  ax = plt.subplot(gs[3, 0]) # row 3
+  ax.plot(ts, np.linalg.norm(evals[:,6:9], axis=1))
+  ax.set_ylabel("acceleration [m/s^2]")
+
+  ax = plt.subplot(gs[4, 0]) # row 4
+  ax.plot(ts, np.linalg.norm(evals[:,9:12], axis=1))
+  ax.set_ylabel("omega [rad/s]")
+
+  ax = plt.subplot(gs[5, 0]) # row 5
+  ax.plot(ts, evals[:,12])
+  ax.set_ylabel("yaw [rad]")
+
+  plt.show()

scripts/uav_trajectory.py

@@ -0,0 +1,106 @@
+#!/usr/bin/env python
+import numpy as np
+
+def normalize(v):
+  norm = np.linalg.norm(v)
+  assert norm > 0
+  return v / norm
+
+
+class Polynomial:
+  def __init__(self, p):
+    self.p = p
+
+  # evaluate a polynomial using horner's rule
+  def eval(self, t):
+    assert t >= 0
+    x = 0.0
+    for i in range(0, len(self.p)):
+      x = x * t + self.p[len(self.p) - 1 - i]
+    return x
+
+  # compute and return derivative
+  def derivative(self):
+    return Polynomial([(i+1) * self.p[i+1] for i in range(0, len(self.p) - 1)])
+
+
+class TrajectoryOutput:
+  def __init__(self):
+    self.pos = None   # position [m]
+    self.vel = None   # velocity [m/s]
+    self.acc = None   # acceleration [m/s^2]
+    self.omega = None # angular velocity [rad/s]
+    self.yaw = None   # yaw angle [rad]
+
+
+# 4d single polynomial piece for x-y-z-yaw, includes duration.
+class Polynomial4D:
+  def __init__(self, duration, px, py, pz, pyaw):
+    self.duration = duration
+    self.px = Polynomial(px)
+    self.py = Polynomial(py)
+    self.pz = Polynomial(pz)
+    self.pyaw = Polynomial(pyaw)
+
+  # compute and return derivative
+  def derivative(self):
+    return Polynomial4D(
+      self.duration,
+      self.px.derivative().p,
+      self.py.derivative().p,
+      self.pz.derivative().p,
+      self.pyaw.derivative().p)
+
+  def eval(self, mass, t):
+    result = TrajectoryOutput()
+    # flat variables
+    result.pos = np.array([self.px.eval(t), self.py.eval(t), self.pz.eval(t)])
+    result.yaw = self.pyaw.eval(t)
+
+    # 1st derivative
+    derivative = self.derivative()
+    result.vel = np.array([derivative.px.eval(t), derivative.py.eval(t), derivative.pz.eval(t)])
+    dyaw = derivative.pyaw.eval(t)
+
+    # 2nd derivative
+    derivative2 = derivative.derivative()
+    result.acc = np.array([derivative2.px.eval(t), derivative2.py.eval(t), derivative2.pz.eval(t)])
+
+    # 3rd derivative
+    derivative3 = derivative2.derivative()
+    jerk = np.array([derivative3.px.eval(t), derivative3.py.eval(t), derivative3.pz.eval(t)])
+
+    thrust = result.acc + np.array([0, 0, 9.81]) # add gravity
+    thrust_mag = mass * np.linalg.norm(thrust)
+
+    z_body = normalize(thrust)
+    x_world = np.array([np.cos(result.yaw), np.sin(result.yaw), 0])
+    y_body = normalize(np.cross(z_body, x_world))
+    x_body = np.cross(y_body, z_body)
+
+    jerk_orth_zbody = jerk - (np.dot(jerk, z_body) * z_body)
+    h_w = mass / thrust_mag * jerk_orth_zbody
+
+    result.omega = np.array([-np.dot(h_w, y_body), np.dot(h_w, x_body), z_body[2] * dyaw])
+    return result
+
+
+class Trajectory:
+  def __init__(self):
+    self.polynomials = None
+    self.duration = None
+
+  def loadcsv(self, filename):
+    data = np.loadtxt(filename, delimiter=",", skiprows=1, usecols=range(33))
+    self.polynomials = [Polynomial4D(row[0], row[1:9], row[9:17], row[17:25], row[25:33]) for row in data]
+    self.duration = np.sum(data[:,0])
+
+  def eval(self, mass, t):
+    assert t >= 0
+    assert t <= self.duration
+
+    current_t = 0.0
+    for p in self.polynomials:
+      if t < current_t + p.duration:
+        return p.eval(mass, t - current_t)
+      current_t = current_t + p.duration