add statistics

scripts/plot_trajectory.py

@@ -21,14 +21,27 @@
     traj.stretchtime(args.stretchtime)
 
   ts = np.arange(0, traj.duration, 0.01)
-  evals = np.empty((len(ts), 13))
+  evals = np.empty((len(ts), 15))
   for t, i in zip(ts, range(0, len(ts))):
     e = traj.eval(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
+    evals[i, 13]   = e.roll
+    evals[i, 14]   = e.pitch
+
+  velocity = np.linalg.norm(evals[:,3:6], axis=1)
+  acceleration = np.linalg.norm(evals[:,6:9], axis=1)
+  omega = np.linalg.norm(evals[:,9:12], axis=1)
+
+  # print stats
+  print("max speed (m/s): ", np.max(velocity))
+  print("max acceleration (m/s^2): ", np.max(acceleration))
+  print("max omega (rad/s): ", np.max(omega))
+  print("max roll (deg): ", np.max(np.degrees(evals[:,13])))
+  print("max pitch (deg): ", np.max(np.degrees(evals[:,14])))
 
   # Create 3x1 sub plots
   gs = gridspec.GridSpec(6, 1)
@@ -38,19 +51,28 @@
   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.plot(ts, velocity)
   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.plot(ts, acceleration)
   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.plot(ts, omega)
   ax.set_ylabel("omega [rad/s]")
 
   ax = plt.subplot(gs[5, 0]) # row 5
-  ax.plot(ts, evals[:,12])
-  ax.set_ylabel("yaw [rad]")
+  ax.plot(ts, np.degrees(evals[:,12]))
+  ax.set_ylabel("yaw [deg]")
+
+  # ax = plt.subplot(gs[6, 0]) # row 5
+  # ax.plot(ts, np.degrees(evals[:,13]))
+  # ax.set_ylabel("roll [deg]")
+
+  # ax = plt.subplot(gs[7, 0]) # row 5
+  # ax.plot(ts, np.degrees(evals[:,14]))
+  # ax.set_ylabel("pitch [deg]")
 
   plt.show()
+

scripts/uav_trajectory.py

@@ -38,6 +38,8 @@ def __init__(self):
     self.acc = None   # acceleration [m/s^2]
     self.omega = None # angular velocity [rad/s]
     self.yaw = None   # yaw angle [rad]
+    self.roll = None  # required roll angle [rad]
+    self.pitch = None # required pitch angle [rad]
 
 
 # 4d single polynomial piece for x-y-z-yaw, includes duration.
@@ -95,6 +97,11 @@ def eval(self, t):
     h_w = jerk_orth_zbody / np.linalg.norm(thrust)
 
     result.omega = np.array([-np.dot(h_w, y_body), np.dot(h_w, x_body), z_body[2] * dyaw])
+
+    # compute required roll/pitch angles
+    result.pitch = np.arcsin(-x_body[2])
+    result.roll = np.arctan2(y_body[2], z_body[2])
+
     return result