Merge pull request #23 from Khaledwahba1994/main

Evaluate polynomials

README.md

@@ -93,6 +93,13 @@ Example:
 python3 ../scripts/plot_trajectory.py traj1.csv
 ```
 
+### Generate PDF and CSV
+A pyhton scirpt can be used to transform the generated csv files with coefficients of the polynomials to (x,y,z) position vector and its derivatives (up to the snap) in a csv format and plot the corresponding values.
+
+```
+python3 scripts/gen_traj.py --traj circle_0.csv --output circle_traj.csv --dt 0.01 --stretchtime 1.0
+```
+
 ### Convert Trajectory to Bezier
 
 This python scripts takes the trajectory generated by genTrajectory, and converts it to a bezier defined between times [0,1]. You should evaluate this bezier for a given time with f(t/duration).

scripts/gen_traj.py

@@ -0,0 +1,119 @@
+import numpy as np
+import matplotlib.pyplot as plt
+from mpl_toolkits.mplot3d import Axes3D
+import matplotlib.gridspec as gridspec
+import argparse
+import os
+from matplotlib.backends.backend_pdf import PdfPages
+import uav_trajectory
+
+if __name__ == "__main__":
+    parser = argparse.ArgumentParser()
+    parser.add_argument("--traj", type=str, help="CSV file containing polynomials")
+    parser.add_argument("--output", type=str, help="CSV file containing pos, vel, acc, jerk, snap")
+    parser.add_argument("--dt", default=0.01, type=float, help="CSV file containing polynomials")
+    parser.add_argument("--stretchtime", default=1.0, type=float, help="stretch time factor (smaller means faster)")
+    args = parser.parse_args()
+
+    traj = uav_trajectory.Trajectory()
+    traj.loadcsv(args.traj)
+
+
+    traj.stretchtime(args.stretchtime)
+
+
+    ts = np.arange(0, traj.duration, args.dt)
+    # t, pos, vel, acc, jerk, snap: 1 + 15 
+    evals = np.empty((len(ts), 15 + 1))  # Additional column for 't'
+    with open(args.output, "w") as f:
+        for t, i in zip(ts, range(0, len(ts))):
+            e = traj.eval(t)
+            evals[i, 0] = t
+            # pos[2] = 0
+            evals[i, 1:4]  = e.pos
+            evals[i, 4:7]  = e.vel
+            evals[i, 7:10] = e.acc
+            evals[i, 10:13] = e.jerk
+            evals[i, 13:16] = e.snap
+        header = "t,posx,posy,posz,velx,vely,velz,accx,accy,accz,jerkx,jerky,jerkz,snapx,snapy,snapz"
+
+        evals_with_header = np.vstack((header.split(','), evals))
+        np.savetxt(f, evals_with_header, delimiter=",", fmt='%s')
+
+
+
+    # Extract position, velocity, and acceleration, jerk, snap data from evals
+    pos = evals[:, 1:4]
+    vel = evals[:, 4:7]
+    acc = evals[:, 7:10]
+    jerk = evals[:, 10:13]
+    snap = evals[:, 13:16]
+
+    # Time steps (using dt from arguments)
+    time_steps = ts
+
+    # Create a PDF file to save the plots
+    pdf_filename = os.path.splitext(args.output)[0] + '.pdf'
+    with PdfPages(pdf_filename) as pdf:  
+        axes = ["x", "y", "z"]
+        # Page 1: pos
+        fig, ax = plt.subplots(3, 1, figsize=(8, 12))
+        for i in range(3):
+            ax[i].plot(time_steps, pos[:, i], label=f'pos[{i}]', color='b')
+            ax[i].set_xlabel('Time (s)')
+            ax[i].set_ylabel(f' {axes[i]}')
+            ax[i].legend()
+            ax[i].grid(True)
+        fig.suptitle('Position')
+        pdf.savefig(fig)
+        plt.close(fig)
+
+        # Page 2: vel
+        fig, ax = plt.subplots(3, 1, figsize=(8, 12))
+        for i in range(3):
+            ax[i].plot(time_steps, vel[:, i], label=f'vel[{i}]', color='b')
+            ax[i].set_xlabel('Time (s)')
+            ax[i].set_ylabel(f' {axes[i]}')
+            ax[i].legend()
+            ax[i].grid(True)
+        fig.suptitle('Velocity')
+        pdf.savefig(fig)
+        plt.close(fig)
+
+        # Page 3: acc
+        fig, ax = plt.subplots(3, 1, figsize=(8, 12))
+        for i in range(3):
+            ax[i].plot(time_steps, acc[:, i], label=f'acc[{i}]', color='b')
+            ax[i].set_xlabel('Time (s)')
+            ax[i].set_ylabel(f' {axes[i]}')
+            ax[i].legend()
+            ax[i].grid(True)
+        fig.suptitle('Acceleration')
+        pdf.savefig(fig)
+        plt.close(fig)
+
+
+        # Page 4: jerk
+        fig, ax = plt.subplots(3, 1, figsize=(8, 12))
+        for i in range(3):
+            ax[i].plot(time_steps, jerk[:, i], label=f'jerk[{i}]', color='b')
+            ax[i].set_xlabel('Time (s)')
+            ax[i].set_ylabel(f' {axes[i]}')
+            ax[i].legend()
+            ax[i].grid(True)
+        fig.suptitle('Jerk')
+        pdf.savefig(fig)
+        plt.close(fig)
+
+        # Page 5: snap
+        fig, ax = plt.subplots(3, 1, figsize=(8, 12))
+        for i in range(3):
+            ax[i].plot(time_steps, snap[:, i], label=f'snap[{i}]', color='b')
+            ax[i].set_xlabel('Time (s)')
+            ax[i].set_ylabel(f' {axes[i]}')
+            ax[i].legend()
+            ax[i].grid(True)
+        fig.suptitle('Snap')
+        pdf.savefig(fig)
+        plt.close(fig)
+

scripts/uav_trajectory.py

@@ -36,6 +36,8 @@ def __init__(self):
     self.pos = None   # position [m]
     self.vel = None   # velocity [m/s]
     self.acc = None   # acceleration [m/s^2]
+    self.jerk = None
+    self.snap = None
     self.omega = None # angular velocity [rad/s]
     self.yaw = None   # yaw angle [rad]
     self.roll = None  # required roll angle [rad]
@@ -88,7 +90,10 @@ def eval(self, t):
     # 3rd derivative
     derivative3 = derivative2.derivative()
     jerk = np.array([derivative3.px.eval(t), derivative3.py.eval(t), derivative3.pz.eval(t)])
+    result.jerk = jerk
 
+    derivative4 = derivative3.derivative()
+    result.snap = np.array([derivative4.px.eval(t), derivative4.py.eval(t), derivative4.pz.eval(t)])
     thrust = result.acc + np.array([0, 0, 9.81]) # add gravity
 
     z_body = normalize(thrust)