diff --git a/demo/F16/F16_waypoint_scene.py b/demo/F16/F16_waypoint_scene.py
new file mode 100644
index 0000000000000000000000000000000000000000..8984fc182bcfbf622e6bd3a7f989986003fd161a
--- /dev/null
+++ b/demo/F16/F16_waypoint_scene.py
@@ -0,0 +1,96 @@
+'''
+Sayan Mitra
+Derived from Stanley Bak's version
+'''
+
+
+from typing import Tuple, List
+
+import numpy as np
+from scipy.integrate import ode
+
+from dryvr_plus_plus.scene_verifier.agents.base_agent import BaseAgent
+from dryvr_plus_plus.scene_verifier.map.lane_map import LaneMap
+from dryvr_plus_plus.scene_verifier.code_parser.pythonparser import EmptyAst
+
+
+import math
+
+from numpy import deg2rad
+import matplotlib.pyplot as plt
+
+from aerobench.run_f16_sim import run_f16_sim
+
+from aerobench.visualize import plot
+
+from waypoint_autopilot import WaypointAutopilot
+
+def main():
+ 'main function'
+
+ ### Initial Conditions ###
+ power = 9 # engine power level (0-10)
+
+ # Default alpha & beta
+ alpha = deg2rad(2.1215) # Trim Angle of Attack (rad)
+ beta = 0 # Side slip angle (rad)
+
+ # Initial Attitude
+ alt = 3800 # altitude (ft)
+ vt = 540 # initial velocity (ft/sec)
+ phi = 0 # Roll angle from wings level (rad)
+ theta = 0 # Pitch angle from nose level (rad)
+ psi = math.pi/8 # Yaw angle from North (rad)
+
+ # Build Initial Condition Vectors
+ # state = [vt, alpha, beta, phi, theta, psi, P, Q, R, pn, pe, h, pow]
+ init = [vt, alpha, beta, phi, theta, psi, 0, 0, 0, 0, 0, alt, power]
+ tmax = 70 # simulation time
+
+ # make waypoint list
+ e_pt = 1000
+ n_pt = 3000
+ h_pt = 4000
+
+ waypoints = [[e_pt, n_pt, h_pt],
+ [e_pt + 2000, n_pt + 5000, h_pt - 100],
+ [e_pt - 2000, n_pt + 15000, h_pt - 250],
+ [e_pt - 500, n_pt + 25000, h_pt]]
+
+ ap = WaypointAutopilot(waypoints, stdout=True)
+
+ step = 1/30
+ extended_states = True
+ res = run_f16_sim(init, tmax, ap, step=step, extended_states=extended_states, integrator_str='rk45')
+
+ print(f"Simulation Completed in {round(res['runtime'], 2)} seconds (extended_states={extended_states})")
+
+ plot.plot_single(res, 'alt', title='Altitude (ft)')
+ filename = 'alt.png'
+ plt.savefig(filename)
+ print(f"Made {filename}")
+
+ plot.plot_overhead(res, waypoints=waypoints)
+ filename = 'overhead.png'
+ plt.savefig(filename)
+ print(f"Made {filename}")
+
+ plot.plot_attitude(res)
+ filename = 'attitude.png'
+ plt.savefig(filename)
+ print(f"Made {filename}")
+
+ # plot inner loop controls + references
+ plot.plot_inner_loop(res)
+ filename = 'inner_loop.png'
+ plt.savefig(filename)
+ print(f"Made {filename}")
+
+ # plot outer loop controls + references
+ plot.plot_outer_loop(res)
+ filename = 'outer_loop.png'
+ plt.savefig(filename)
+ print(f"Made {filename}")
+
+if __name__ == '__main__':
+ main()
diff --git a/demo/F16/aerobench/examples/waypoint/F16_waypoint_scene.py b/demo/F16/aerobench/examples/waypoint/F16_waypoint_scene.py
new file mode 100644
index 0000000000000000000000000000000000000000..8984fc182bcfbf622e6bd3a7f989986003fd161a
--- /dev/null
+++ b/demo/F16/aerobench/examples/waypoint/F16_waypoint_scene.py
@@ -0,0 +1,96 @@
+'''
+Sayan Mitra
+Derived from Stanley Bak's version
+'''
+
+
+from typing import Tuple, List
+
+import numpy as np
+from scipy.integrate import ode
+
+from dryvr_plus_plus.scene_verifier.agents.base_agent import BaseAgent
+from dryvr_plus_plus.scene_verifier.map.lane_map import LaneMap
+from dryvr_plus_plus.scene_verifier.code_parser.pythonparser import EmptyAst
+
+
+import math
+
+from numpy import deg2rad
+import matplotlib.pyplot as plt
+
+from aerobench.run_f16_sim import run_f16_sim
+
+from aerobench.visualize import plot
+
+from waypoint_autopilot import WaypointAutopilot
+
+def main():
+ 'main function'
+
+ ### Initial Conditions ###
+ power = 9 # engine power level (0-10)
+
+ # Default alpha & beta
+ alpha = deg2rad(2.1215) # Trim Angle of Attack (rad)
+ beta = 0 # Side slip angle (rad)
+
+ # Initial Attitude
+ alt = 3800 # altitude (ft)
+ vt = 540 # initial velocity (ft/sec)
+ phi = 0 # Roll angle from wings level (rad)
+ theta = 0 # Pitch angle from nose level (rad)
+ psi = math.pi/8 # Yaw angle from North (rad)
+
+ # Build Initial Condition Vectors
+ # state = [vt, alpha, beta, phi, theta, psi, P, Q, R, pn, pe, h, pow]
+ init = [vt, alpha, beta, phi, theta, psi, 0, 0, 0, 0, 0, alt, power]
+ tmax = 70 # simulation time
+
+ # make waypoint list
+ e_pt = 1000
+ n_pt = 3000
+ h_pt = 4000
+
+ waypoints = [[e_pt, n_pt, h_pt],
+ [e_pt + 2000, n_pt + 5000, h_pt - 100],
+ [e_pt - 2000, n_pt + 15000, h_pt - 250],
+ [e_pt - 500, n_pt + 25000, h_pt]]
+
+ ap = WaypointAutopilot(waypoints, stdout=True)
+
+ step = 1/30
+ extended_states = True
+ res = run_f16_sim(init, tmax, ap, step=step, extended_states=extended_states, integrator_str='rk45')
+
+ print(f"Simulation Completed in {round(res['runtime'], 2)} seconds (extended_states={extended_states})")
+
+ plot.plot_single(res, 'alt', title='Altitude (ft)')
+ filename = 'alt.png'
+ plt.savefig(filename)
+ print(f"Made {filename}")
+
+ plot.plot_overhead(res, waypoints=waypoints)
+ filename = 'overhead.png'
+ plt.savefig(filename)
+ print(f"Made {filename}")
+
+ plot.plot_attitude(res)
+ filename = 'attitude.png'
+ plt.savefig(filename)
+ print(f"Made {filename}")
+
+ # plot inner loop controls + references
+ plot.plot_inner_loop(res)
+ filename = 'inner_loop.png'
+ plt.savefig(filename)
+ print(f"Made {filename}")
+
+ # plot outer loop controls + references
+ plot.plot_outer_loop(res)
+ filename = 'outer_loop.png'
+ plt.savefig(filename)
+ print(f"Made {filename}")
+
+if __name__ == '__main__':
+ main()
diff --git a/demo/F16/aerobench/examples/waypoint/run_u_turn.py b/demo/F16/aerobench/examples/waypoint/run_u_turn.py
new file mode 100644
index 0000000000000000000000000000000000000000..f7f77701f1497be07008a68aeb41bc34d3286825
--- /dev/null
+++ b/demo/F16/aerobench/examples/waypoint/run_u_turn.py
@@ -0,0 +1,81 @@
+'''
+Stanley Bak
+
+should match 'u_turn' scenario from matlab version
+'''
+
+import math
+
+from numpy import deg2rad
+import matplotlib.pyplot as plt
+
+from aerobench.run_f16_sim import run_f16_sim
+
+from aerobench.visualize import plot
+
+from waypoint_autopilot import WaypointAutopilot
+
+def main():
+ 'main function'
+
+ ### Initial Conditions ###
+ power = 9 # engine power level (0-10)
+
+ # Default alpha & beta
+ alpha = deg2rad(2.1215) # Trim Angle of Attack (rad)
+ beta = 0 # Side slip angle (rad)
+
+ # Initial Attitude
+ alt = 1500 # altitude (ft)
+ vt = 540 # initial velocity (ft/sec)
+ phi = 0 # Roll angle from wings level (rad)
+ theta = 0 # Pitch angle from nose level (rad)
+ psi = 0 # Yaw angle from North (rad)
+
+ # Build Initial Condition Vectors
+ # state = [vt, alpha, beta, phi, theta, psi, P, Q, R, pn, pe, h, pow]
+ init = [vt, alpha, beta, phi, theta, psi, 0, 0, 0, 0, 0, alt, power]
+ tmax = 150 # simulation time
+
+ # make waypoint list
+ waypoints = [[-5000, -7500, alt],
+ [-15000, -7500, alt],
+ [-20000, 0, alt+500]]
+
+ ap = WaypointAutopilot(waypoints, stdout=True)
+
+ step = 1/30
+ extended_states = True
+ res = run_f16_sim(init, tmax, ap, step=step, extended_states=extended_states, integrator_str='rk45')
+
+ print(f"Simulation Completed in {round(res['runtime'], 2)} seconds (extended_states={extended_states})")
+
+ plot.plot_single(res, 'alt', title='Altitude (ft)')
+ filename = 'alt.png'
+ plt.savefig(filename)
+ print(f"Made {filename}")
+
+ plot.plot_overhead(res, waypoints=waypoints)
+ filename = 'overhead.png'
+ plt.savefig(filename)
+ print(f"Made {filename}")
+
+ plot.plot_attitude(res)
+ filename = 'attitude.png'
+ plt.savefig(filename)
+ print(f"Made {filename}")
+
+ # plot inner loop controls + references
+ plot.plot_inner_loop(res)
+ filename = 'inner_loop.png'
+ plt.savefig(filename)
+ print(f"Made {filename}")
+
+ # plot outer loop controls + references
+ plot.plot_outer_loop(res)
+ filename = 'outer_loop.png'
+ plt.savefig(filename)
+ print(f"Made {filename}")
+
+if __name__ == '__main__':
+ main()
diff --git a/demo/F16/aerobench/examples/waypoint/run_waypoint.py b/demo/F16/aerobench/examples/waypoint/run_waypoint.py
new file mode 100644
index 0000000000000000000000000000000000000000..e2952a6b5034a24e045e8ffba2ade2db21a6f38b
--- /dev/null
+++ b/demo/F16/aerobench/examples/waypoint/run_waypoint.py
@@ -0,0 +1,86 @@
+'''
+Stanley Bak
+
+should match 'waypoint' scenario from matlab version
+'''
+
+import math
+
+from numpy import deg2rad
+import matplotlib.pyplot as plt
+
+from aerobench.run_f16_sim import run_f16_sim
+
+from aerobench.visualize import plot
+
+from waypoint_autopilot import WaypointAutopilot
+
+def main():
+ 'main function'
+
+ ### Initial Conditions ###
+ power = 9 # engine power level (0-10)
+
+ # Default alpha & beta
+ alpha = deg2rad(2.1215) # Trim Angle of Attack (rad)
+ beta = 0 # Side slip angle (rad)
+
+ # Initial Attitude
+ alt = 3800 # altitude (ft)
+ vt = 540 # initial velocity (ft/sec)
+ phi = 0 # Roll angle from wings level (rad)
+ theta = 0 # Pitch angle from nose level (rad)
+ psi = math.pi/8 # Yaw angle from North (rad)
+
+ # Build Initial Condition Vectors
+ # state = [vt, alpha, beta, phi, theta, psi, P, Q, R, pn, pe, h, pow]
+ init = [vt, alpha, beta, phi, theta, psi, 0, 0, 0, 0, 0, alt, power]
+ tmax = 70 # simulation time
+
+ # make waypoint list
+ e_pt = 1000
+ n_pt = 3000
+ h_pt = 4000
+
+ waypoints = [[e_pt, n_pt, h_pt],
+ [e_pt + 2000, n_pt + 5000, h_pt - 100],
+ [e_pt - 2000, n_pt + 15000, h_pt - 250],
+ [e_pt - 500, n_pt + 25000, h_pt]]
+
+ ap = WaypointAutopilot(waypoints, stdout=True)
+
+ step = 1/30
+ extended_states = True
+ res = run_f16_sim(init, tmax, ap, step=step, extended_states=extended_states, integrator_str='rk45')
+
+ print(f"Simulation Completed in {round(res['runtime'], 2)} seconds (extended_states={extended_states})")
+
+ plot.plot_single(res, 'alt', title='Altitude (ft)')
+ filename = 'alt.png'
+ plt.savefig(filename)
+ print(f"Made {filename}")
+
+ plot.plot_overhead(res, waypoints=waypoints)
+ filename = 'overhead.png'
+ plt.savefig(filename)
+ print(f"Made {filename}")
+
+ plot.plot_attitude(res)
+ filename = 'attitude.png'
+ plt.savefig(filename)
+ print(f"Made {filename}")
+
+ # plot inner loop controls + references
+ plot.plot_inner_loop(res)
+ filename = 'inner_loop.png'
+ plt.savefig(filename)
+ print(f"Made {filename}")
+
+ # plot outer loop controls + references
+ plot.plot_outer_loop(res)
+ filename = 'outer_loop.png'
+ plt.savefig(filename)
+ print(f"Made {filename}")
+
+if __name__ == '__main__':
+ main()
diff --git a/demo/F16/aerobench/examples/waypoint/waypoint_autopilot.py b/demo/F16/aerobench/examples/waypoint/waypoint_autopilot.py
new file mode 100644
index 0000000000000000000000000000000000000000..0c9575ff51826eb2452f965af05e6cf155fa2c11
--- /dev/null
+++ b/demo/F16/aerobench/examples/waypoint/waypoint_autopilot.py
@@ -0,0 +1,307 @@
+'''waypoint autopilot
+
+ported from matlab v2
+'''
+
+from math import pi, atan2, sqrt, sin, cos, asin
+
+import numpy as np
+
+from aerobench.highlevel.autopilot import Autopilot
+from aerobench.util import StateIndex
+from aerobench.lowlevel.low_level_controller import LowLevelController
+
+class WaypointAutopilot(Autopilot):
+ '''waypoint follower autopilot'''
+
+ def __init__(self, waypoints, gain_str='old', stdout=False):
+ 'waypoints is a list of 3-tuples'
+
+ self.stdout = stdout
+ self.waypoints = waypoints
+ self.waypoint_index = 0
+
+ # waypoint config
+ self.cfg_slant_range_threshold = 250
+
+ # default control when not waypoint tracking
+ self.cfg_u_ol_default = (0, 0, 0, 0.3)
+
+ # control config
+ # Gains for speed control
+ self.cfg_k_vt = 0.25
+ self.cfg_airspeed = 550
+
+ # Gains for altitude tracking
+ self.cfg_k_alt = 0.005
+ self.cfg_k_h_dot = 0.02
+
+ # Gains for heading tracking
+ self.cfg_k_prop_psi = 5
+ self.cfg_k_der_psi = 0.5
+
+ # Gains for roll tracking
+ self.cfg_k_prop_phi = 0.75
+ self.cfg_k_der_phi = 0.5
+ self.cfg_max_bank_deg = 65 # maximum bank angle setpoint
+ # v2 was 0.5, 0.9
+
+ # Ranges for Nz
+ self.cfg_max_nz_cmd = 4
+ self.cfg_min_nz_cmd = -1
+
+ self.done_time = 0.0
+
+ llc = LowLevelController(gain_str=gain_str)
+
+ Autopilot.__init__(self, 'Waypoint 1', llc=llc)
+
+ def log(self, s):
+ 'print to terminal if stdout is true'
+
+ if self.stdout:
+ print(s)
+
+ def get_u_ref(self, _t, x_f16):
+ '''get the reference input signals'''
+
+ if self.mode != "Done":
+ psi_cmd = self.get_waypoint_data(x_f16)[0]
+
+ # Get desired roll angle given desired heading
+ phi_cmd = self.get_phi_to_track_heading(x_f16, psi_cmd)
+ ps_cmd = self.track_roll_angle(x_f16, phi_cmd)
+
+ nz_cmd = self.track_altitude(x_f16)
+ throttle = self.track_airspeed(x_f16)
+ else:
+ # Waypoint Following complete: fly level.
+ throttle = self.track_airspeed(x_f16)
+ ps_cmd = self.track_roll_angle(x_f16, 0)
+ nz_cmd = self.track_altitude_wings_level(x_f16)
+
+ # trim to limits
+ nz_cmd = max(self.cfg_min_nz_cmd, min(self.cfg_max_nz_cmd, nz_cmd))
+ throttle = max(min(throttle, 1), 0)
+
+ # Create reference vector
+ rv = [nz_cmd, ps_cmd, 0, throttle]
+
+ return rv
+
+ def track_altitude(self, x_f16):
+ 'get nz to track altitude, taking turning into account'
+
+ h_cmd = self.waypoints[self.waypoint_index][2]
+
+ h = x_f16[StateIndex.ALT]
+ phi = x_f16[StateIndex.PHI]
+
+ # Calculate altitude error (positive => below target alt)
+ h_error = h_cmd - h
+ nz_alt = self.track_altitude_wings_level(x_f16)
+ nz_roll = get_nz_for_level_turn_ol(x_f16)
+
+ if h_error > 0:
+ # Ascend wings level or banked
+ nz = nz_alt + nz_roll
+ elif abs(phi) < np.deg2rad(15):
+ # Descend wings (close enough to) level
+ nz = nz_alt + nz_roll
+ else:
+ # Descend in bank (no negative Gs)
+ nz = max(0, nz_alt + nz_roll)
+
+ return nz
+
+ def get_phi_to_track_heading(self, x_f16, psi_cmd):
+ 'get phi from psi_cmd'
+
+ # PD Control on heading angle using phi_cmd as control
+
+ # Pull out important variables for ease of use
+ psi = wrap_to_pi(x_f16[StateIndex.PSI])
+ r = x_f16[StateIndex.R]
+
+ # Calculate PD control
+ psi_err = wrap_to_pi(psi_cmd - psi)
+
+ phi_cmd = psi_err * self.cfg_k_prop_psi - r * self.cfg_k_der_psi
+
+ # Bound to acceptable bank angles:
+ max_bank_rad = np.deg2rad(self.cfg_max_bank_deg)
+
+ phi_cmd = min(max(phi_cmd, -max_bank_rad), max_bank_rad)
+
+ return phi_cmd
+
+ def track_roll_angle(self, x_f16, phi_cmd):
+ 'get roll angle command (ps_cmd)'
+
+ # PD control on roll angle using stability roll rate
+
+ # Pull out important variables for ease of use
+ phi = x_f16[StateIndex.PHI]
+ p = x_f16[StateIndex.P]
+
+ # Calculate PD control
+ ps = (phi_cmd-phi) * self.cfg_k_prop_phi - p * self.cfg_k_der_phi
+
+ return ps
+
+ def track_airspeed(self, x_f16):
+ 'get throttle command'
+
+ vt_cmd = self.cfg_airspeed
+
+ # Proportional control on airspeed using throttle
+ throttle = self.cfg_k_vt * (vt_cmd - x_f16[StateIndex.VT])
+
+ return throttle
+
+ def track_altitude_wings_level(self, x_f16):
+ 'get nz to track altitude'
+
+ i = self.waypoint_index if self.waypoint_index < len(self.waypoints) else -1
+
+ h_cmd = self.waypoints[i][2]
+
+ vt = x_f16[StateIndex.VT]
+ h = x_f16[StateIndex.ALT]
+
+ # Proportional-Derivative Control
+ h_error = h_cmd - h
+ gamma = get_path_angle(x_f16)
+ h_dot = vt * sin(gamma) # Calculated, not differentiated
+
+ # Calculate Nz command
+ nz = self.cfg_k_alt*h_error - self.cfg_k_h_dot*h_dot
+
+ return nz
+
+ def is_finished(self, t, x_f16):
+ 'is the maneuver done?'
+
+ rv = self.waypoint_index >= len(self.waypoints) and self.done_time + 5.0 < t
+
+ return rv
+
+ def advance_discrete_mode(self, t, x_f16):
+ '''
+ advance the discrete state based on the current aircraft state. Returns True iff the discrete state
+ has changed.
+ '''
+
+ if self.waypoint_index < len(self.waypoints):
+ slant_range = self.get_waypoint_data(x_f16)[-1]
+
+ if slant_range < self.cfg_slant_range_threshold:
+ self.waypoint_index += 1
+
+ if self.waypoint_index >= len(self.waypoints):
+ self.done_time = t
+
+ premode = self.mode
+
+ if self.waypoint_index >= len(self.waypoints):
+ self.mode = 'Done'
+ else:
+ self.mode = f'Waypoint {self.waypoint_index + 1}'
+
+ rv = premode != self.mode
+
+ if rv:
+ self.log(f"Waypoint transition {premode} -> {self.mode} at time {t}")
+
+ return rv
+
+ def get_waypoint_data(self, x_f16):
+ '''returns current waypoint data tuple based on the current waypoint:
+
+ (heading, inclination, horiz_range, vert_range, slant_range)
+
+ heading = heading to tgt, equivalent to psi (rad)
+ inclination = polar angle to tgt, equivalent to theta (rad)
+ horiz_range = horizontal range to tgt (ft)
+ vert_range = vertical range to tgt (ft)
+ slant_range = total range to tgt (ft)
+ '''
+
+ waypoint = self.waypoints[self.waypoint_index]
+
+ e_pos = x_f16[StateIndex.POSE]
+ n_pos = x_f16[StateIndex.POSN]
+ alt = x_f16[StateIndex.ALT]
+
+ delta = [waypoint[i] - [e_pos, n_pos, alt][i] for i in range(3)]
+
+ _, inclination, slant_range = cart2sph(delta)
+
+ heading = wrap_to_pi(pi/2 - atan2(delta[1], delta[0]))
+
+ horiz_range = np.linalg.norm(delta[0:2])
+ vert_range = np.linalg.norm(delta[2])
+
+ return heading, inclination, horiz_range, vert_range, slant_range
+
+def get_nz_for_level_turn_ol(x_f16):
+ 'get nz to do a level turn'
+
+ # Pull g's to maintain altitude during bank based on trig
+
+ # Calculate theta
+ phi = x_f16[StateIndex.PHI]
+
+ if abs(phi): # if cos(phi) ~= 0, basically
+ nz = 1 / cos(phi) - 1 # Keeps plane at altitude
+ else:
+ nz = 0
+
+ return nz
+
+def get_path_angle(x_f16):
+ 'get the path angle gamma'
+
+ alpha = x_f16[StateIndex.ALPHA] # AoA (rad)
+ beta = x_f16[StateIndex.BETA] # Sideslip (rad)
+ phi = x_f16[StateIndex.PHI] # Roll anle (rad)
+ theta = x_f16[StateIndex.THETA] # Pitch angle (rad)
+
+ gamma = asin((cos(alpha)*sin(theta)- \
+ sin(alpha)*cos(theta)*cos(phi))*cos(beta) - \
+ (cos(theta)*sin(phi))*sin(beta))
+
+ return gamma
+
+def wrap_to_pi(psi_rad):
+ '''handle angle wrapping
+
+ returns equivelent angle in range [-pi, pi]
+ '''
+
+ rv = psi_rad % (2 * pi)
+
+ if rv > pi:
+ rv -= 2 * pi
+
+ return rv
+
+def cart2sph(pt3d):
+ '''
+ Cartesian to spherical coordinates
+
+ returns az, elev, r
+ '''
+
+ x, y, z = pt3d
+
+ h = sqrt(x*x + y*y)
+ r = sqrt(h*h + z*z)
+
+ elev = atan2(z, h)
+ az = atan2(y, x)
+
+ return az, elev, r
+
+if __name__ == '__main__':
+ print("Autopulot script not meant to be run directly.")
diff --git a/demo/F16/aerobench/run_f16_sim.py b/demo/F16/aerobench/run_f16_sim.py
new file mode 100644
index 0000000000000000000000000000000000000000..027303baa88be7b27303d02bdc1dc9188ac804a9
--- /dev/null
+++ b/demo/F16/aerobench/run_f16_sim.py
@@ -0,0 +1,227 @@
+'''
+Stanley Bak
+run_f16_sim python version
+'''
+
+import time
+
+import numpy as np
+from scipy.integrate import RK45
+
+from aerobench.highlevel.controlled_f16 import controlled_f16
+from aerobench.util import get_state_names, Euler
+
+from dryvr_plus_plus.scene_verifier.agents.base_agent import BaseAgent
+from dryvr_plus_plus.scene_verifier.map.lane_map import LaneMap
+from dryvr_plus_plus.scene_verifier.code_parser.pythonparser import EmptyAst
+
+
+class F16Agent(BaseAgent):
+ '''Dynamics of an F16 aircraft
+ derived from Stanley Bak's python library'''
+ def __init__(self, id, code = None, file_name = None):
+ '''Contructor for tha agent
+ EXACTLY one of the following should be given
+ file_name: name of the controller
+ code: pyhton string ddefning the controller
+ '''
+ # Calling the constructor of tha base class
+ super().__init__(id, code, file_name)
+
+def run_f16_sim(initial_state, tmax, ap, step=1/30, extended_states=False, model_str='morelli',
+ integrator_str='rk45', v2_integrators=False):
+ '''Simulates and analyzes autonomous F-16 maneuvers
+
+ if multiple aircraft are to be simulated at the same time,
+ initial_state should be the concatenated full (including integrators) initial state.
+
+ returns a dict with the following keys:
+
+ 'status': integration status, should be 'finished' if no errors, or 'autopilot finished'
+ 'times': time history
+ 'states': state history at each time step
+ 'modes': mode history at each time step
+
+ if extended_states was True, result also includes:
+ 'xd_list' - derivative at each time step
+ 'ps_list' - ps at each time step
+ 'Nz_list' - Nz at each time step
+ 'Ny_r_list' - Ny_r at each time step
+ 'u_list' - input at each time step, input is 7-tuple: throt, ele, ail, rud, Nz_ref, ps_ref, Ny_r_ref
+ These are tuples if multiple aircraft are used
+ '''
+
+ start = time.perf_counter()
+
+ initial_state = np.array(initial_state, dtype=float)
+ llc = ap.llc
+
+ num_vars = len(get_state_names()) + llc.get_num_integrators()
+
+ if initial_state.size < num_vars:
+ # append integral error states to state vector
+ x0 = np.zeros(num_vars)
+ x0[:initial_state.shape[0]] = initial_state
+ else:
+ x0 = initial_state
+
+ assert x0.size % num_vars == 0, f"expected initial state ({x0.size} vars) to be multiple of {num_vars} vars"
+
+ # run the numerical simulation
+ times = [0]
+ states = [x0]
+
+ # mode can change at time 0
+ ap.advance_discrete_mode(times[-1], states[-1])
+
+ modes = [ap.mode]
+
+ if extended_states:
+ xd, u, Nz, ps, Ny_r = get_extended_states(ap, times[-1], states[-1], model_str, v2_integrators)
+
+ xd_list = [xd]
+ u_list = [u]
+ Nz_list = [Nz]
+ ps_list = [ps]
+ Ny_r_list = [Ny_r]
+
+ der_func = make_der_func(ap, model_str, v2_integrators)
+
+ if integrator_str == 'rk45':
+ integrator_class = RK45
+ kwargs = {}
+ else:
+ assert integrator_str == 'euler'
+ integrator_class = Euler
+ kwargs = {'step': step}
+
+ # note: fixed_step argument is unused by rk45, used with euler
+ integrator = integrator_class(der_func, times[-1], states[-1], tmax, **kwargs)
+
+ while integrator.status == 'running':
+ integrator.step()
+
+ if integrator.t >= times[-1] + step:
+ dense_output = integrator.dense_output()
+
+ while integrator.t >= times[-1] + step:
+ t = times[-1] + step
+ #print(f"{round(t, 2)} / {tmax}")
+
+ times.append(t)
+ states.append(dense_output(t))
+
+ updated = ap.advance_discrete_mode(times[-1], states[-1])
+ modes.append(ap.mode)
+
+ # re-run dynamics function at current state to get non-state variables
+ if extended_states:
+ xd, u, Nz, ps, Ny_r = get_extended_states(ap, times[-1], states[-1], model_str, v2_integrators)
+
+ xd_list.append(xd)
+ u_list.append(u)
+
+ Nz_list.append(Nz)
+ ps_list.append(ps)
+ Ny_r_list.append(Ny_r)
+
+ if ap.is_finished(times[-1], states[-1]):
+ # this both causes the outer loop to exit and sets res['status'] appropriately
+ integrator.status = 'autopilot finished'
+ break
+
+ if updated:
+ # re-initialize the integration class on discrete mode switches
+ integrator = integrator_class(der_func, times[-1], states[-1], tmax, **kwargs)
+ break
+
+ assert 'finished' in integrator.status
+
+ res = {}
+ res['status'] = integrator.status
+ res['times'] = times
+ res['states'] = np.array(states, dtype=float)
+ res['modes'] = modes
+
+ if extended_states:
+ res['xd_list'] = xd_list
+ res['ps_list'] = ps_list
+ res['Nz_list'] = Nz_list
+ res['Ny_r_list'] = Ny_r_list
+ res['u_list'] = u_list
+
+ res['runtime'] = time.perf_counter() - start
+
+ return res
+
+def make_der_func(ap, model_str, v2_integrators):
+ 'make the combined derivative function for integration'
+
+ def der_func(t, full_state):
+ 'derivative function, generalized for multiple aircraft'
+
+ u_refs = ap.get_checked_u_ref(t, full_state)
+
+ num_aircraft = u_refs.size // 4
+ num_vars = len(get_state_names()) + ap.llc.get_num_integrators()
+ assert full_state.size // num_vars == num_aircraft
+
+ xds = []
+
+ for i in range(num_aircraft):
+ state = full_state[num_vars*i:num_vars*(i+1)]
+ u_ref = u_refs[4*i:4*(i+1)]
+
+ xd = controlled_f16(t, state, u_ref, ap.llc, model_str, v2_integrators)[0]
+ xds.append(xd)
+
+ rv = np.hstack(xds)
+
+ return rv
+
+ return der_func
+
+def get_extended_states(ap, t, full_state, model_str, v2_integrators):
+ '''get xd, u, Nz, ps, Ny_r at the current time / state
+
+ returns tuples if more than one aircraft
+ '''
+
+ llc = ap.llc
+ num_vars = len(get_state_names()) + llc.get_num_integrators()
+ num_aircraft = full_state.size // num_vars
+
+ xd_tup = []
+ u_tup = []
+ Nz_tup = []
+ ps_tup = []
+ Ny_r_tup = []
+
+ u_refs = ap.get_checked_u_ref(t, full_state)
+
+ for i in range(num_aircraft):
+ state = full_state[num_vars*i:num_vars*(i+1)]
+ u_ref = u_refs[4*i:4*(i+1)]
+
+ xd, u, Nz, ps, Ny_r = controlled_f16(t, state, u_ref, llc, model_str, v2_integrators)
+
+ xd_tup.append(xd)
+ u_tup.append(u)
+ Nz_tup.append(Nz)
+ ps_tup.append(ps)
+ Ny_r_tup.append(Ny_r)
+
+ if num_aircraft == 1:
+ rv_xd = xd_tup[0]
+ rv_u = u_tup[0]
+ rv_Nz = Nz_tup[0]
+ rv_ps = ps_tup[0]
+ rv_Ny_r = Ny_r_tup[0]
+ else:
+ rv_xd = tuple(xd_tup)
+ rv_u = tuple(u_tup)
+ rv_Nz = tuple(Nz_tup)
+ rv_ps = tuple(ps_tup)
+ rv_Ny_r = tuple(Ny_r_tup)
+
+ return rv_xd, rv_u, rv_Nz, rv_ps, rv_Ny_r
diff --git a/demo/F16/alt.png b/demo/F16/alt.png
new file mode 100644
index 0000000000000000000000000000000000000000..2ce1015d7fd9d284e062ff5b3ae13a9a994d2c7f
Binary files /dev/null and b/demo/F16/alt.png differ
diff --git a/demo/F16/attitude.png b/demo/F16/attitude.png
new file mode 100644
index 0000000000000000000000000000000000000000..e45ef4cfde0c619b767e5ffc6e142b6255aaa41f
Binary files /dev/null and b/demo/F16/attitude.png differ
diff --git a/demo/F16/inner_loop.png b/demo/F16/inner_loop.png
new file mode 100644
index 0000000000000000000000000000000000000000..35a1669f956d0fa4cc17af0cd5c16927d0508bb7
Binary files /dev/null and b/demo/F16/inner_loop.png differ
diff --git a/demo/F16/outer_loop.png b/demo/F16/outer_loop.png
new file mode 100644
index 0000000000000000000000000000000000000000..7689d3aa137f10331f44a1c9db6faa2c75a9bec4
Binary files /dev/null and b/demo/F16/outer_loop.png differ
diff --git a/demo/F16/overhead.png b/demo/F16/overhead.png
new file mode 100644
index 0000000000000000000000000000000000000000..349b8404228524ce0096682069cef50f7cfab78c
Binary files /dev/null and b/demo/F16/overhead.png differ
diff --git a/demo/F16/waypoint_autopilot.py b/demo/F16/waypoint_autopilot.py
new file mode 100644
index 0000000000000000000000000000000000000000..0c9575ff51826eb2452f965af05e6cf155fa2c11
--- /dev/null
+++ b/demo/F16/waypoint_autopilot.py
@@ -0,0 +1,307 @@
+'''waypoint autopilot
+
+ported from matlab v2
+'''
+
+from math import pi, atan2, sqrt, sin, cos, asin
+
+import numpy as np
+
+from aerobench.highlevel.autopilot import Autopilot
+from aerobench.util import StateIndex
+from aerobench.lowlevel.low_level_controller import LowLevelController
+
+class WaypointAutopilot(Autopilot):
+ '''waypoint follower autopilot'''
+
+ def __init__(self, waypoints, gain_str='old', stdout=False):
+ 'waypoints is a list of 3-tuples'
+
+ self.stdout = stdout
+ self.waypoints = waypoints
+ self.waypoint_index = 0
+
+ # waypoint config
+ self.cfg_slant_range_threshold = 250
+
+ # default control when not waypoint tracking
+ self.cfg_u_ol_default = (0, 0, 0, 0.3)
+
+ # control config
+ # Gains for speed control
+ self.cfg_k_vt = 0.25
+ self.cfg_airspeed = 550
+
+ # Gains for altitude tracking
+ self.cfg_k_alt = 0.005
+ self.cfg_k_h_dot = 0.02
+
+ # Gains for heading tracking
+ self.cfg_k_prop_psi = 5
+ self.cfg_k_der_psi = 0.5
+
+ # Gains for roll tracking
+ self.cfg_k_prop_phi = 0.75
+ self.cfg_k_der_phi = 0.5
+ self.cfg_max_bank_deg = 65 # maximum bank angle setpoint
+ # v2 was 0.5, 0.9
+
+ # Ranges for Nz
+ self.cfg_max_nz_cmd = 4
+ self.cfg_min_nz_cmd = -1
+
+ self.done_time = 0.0
+
+ llc = LowLevelController(gain_str=gain_str)
+
+ Autopilot.__init__(self, 'Waypoint 1', llc=llc)
+
+ def log(self, s):
+ 'print to terminal if stdout is true'
+
+ if self.stdout:
+ print(s)
+
+ def get_u_ref(self, _t, x_f16):
+ '''get the reference input signals'''
+
+ if self.mode != "Done":
+ psi_cmd = self.get_waypoint_data(x_f16)[0]
+
+ # Get desired roll angle given desired heading
+ phi_cmd = self.get_phi_to_track_heading(x_f16, psi_cmd)
+ ps_cmd = self.track_roll_angle(x_f16, phi_cmd)
+
+ nz_cmd = self.track_altitude(x_f16)
+ throttle = self.track_airspeed(x_f16)
+ else:
+ # Waypoint Following complete: fly level.
+ throttle = self.track_airspeed(x_f16)
+ ps_cmd = self.track_roll_angle(x_f16, 0)
+ nz_cmd = self.track_altitude_wings_level(x_f16)
+
+ # trim to limits
+ nz_cmd = max(self.cfg_min_nz_cmd, min(self.cfg_max_nz_cmd, nz_cmd))
+ throttle = max(min(throttle, 1), 0)
+
+ # Create reference vector
+ rv = [nz_cmd, ps_cmd, 0, throttle]
+
+ return rv
+
+ def track_altitude(self, x_f16):
+ 'get nz to track altitude, taking turning into account'
+
+ h_cmd = self.waypoints[self.waypoint_index][2]
+
+ h = x_f16[StateIndex.ALT]
+ phi = x_f16[StateIndex.PHI]
+
+ # Calculate altitude error (positive => below target alt)
+ h_error = h_cmd - h
+ nz_alt = self.track_altitude_wings_level(x_f16)
+ nz_roll = get_nz_for_level_turn_ol(x_f16)
+
+ if h_error > 0:
+ # Ascend wings level or banked
+ nz = nz_alt + nz_roll
+ elif abs(phi) < np.deg2rad(15):
+ # Descend wings (close enough to) level
+ nz = nz_alt + nz_roll
+ else:
+ # Descend in bank (no negative Gs)
+ nz = max(0, nz_alt + nz_roll)
+
+ return nz
+
+ def get_phi_to_track_heading(self, x_f16, psi_cmd):
+ 'get phi from psi_cmd'
+
+ # PD Control on heading angle using phi_cmd as control
+
+ # Pull out important variables for ease of use
+ psi = wrap_to_pi(x_f16[StateIndex.PSI])
+ r = x_f16[StateIndex.R]
+
+ # Calculate PD control
+ psi_err = wrap_to_pi(psi_cmd - psi)
+
+ phi_cmd = psi_err * self.cfg_k_prop_psi - r * self.cfg_k_der_psi
+
+ # Bound to acceptable bank angles:
+ max_bank_rad = np.deg2rad(self.cfg_max_bank_deg)
+
+ phi_cmd = min(max(phi_cmd, -max_bank_rad), max_bank_rad)
+
+ return phi_cmd
+
+ def track_roll_angle(self, x_f16, phi_cmd):
+ 'get roll angle command (ps_cmd)'
+
+ # PD control on roll angle using stability roll rate
+
+ # Pull out important variables for ease of use
+ phi = x_f16[StateIndex.PHI]
+ p = x_f16[StateIndex.P]
+
+ # Calculate PD control
+ ps = (phi_cmd-phi) * self.cfg_k_prop_phi - p * self.cfg_k_der_phi
+
+ return ps
+
+ def track_airspeed(self, x_f16):
+ 'get throttle command'
+
+ vt_cmd = self.cfg_airspeed
+
+ # Proportional control on airspeed using throttle
+ throttle = self.cfg_k_vt * (vt_cmd - x_f16[StateIndex.VT])
+
+ return throttle
+
+ def track_altitude_wings_level(self, x_f16):
+ 'get nz to track altitude'
+
+ i = self.waypoint_index if self.waypoint_index < len(self.waypoints) else -1
+
+ h_cmd = self.waypoints[i][2]
+
+ vt = x_f16[StateIndex.VT]
+ h = x_f16[StateIndex.ALT]
+
+ # Proportional-Derivative Control
+ h_error = h_cmd - h
+ gamma = get_path_angle(x_f16)
+ h_dot = vt * sin(gamma) # Calculated, not differentiated
+
+ # Calculate Nz command
+ nz = self.cfg_k_alt*h_error - self.cfg_k_h_dot*h_dot
+
+ return nz
+
+ def is_finished(self, t, x_f16):
+ 'is the maneuver done?'
+
+ rv = self.waypoint_index >= len(self.waypoints) and self.done_time + 5.0 < t
+
+ return rv
+
+ def advance_discrete_mode(self, t, x_f16):
+ '''
+ advance the discrete state based on the current aircraft state. Returns True iff the discrete state
+ has changed.
+ '''
+
+ if self.waypoint_index < len(self.waypoints):
+ slant_range = self.get_waypoint_data(x_f16)[-1]
+
+ if slant_range < self.cfg_slant_range_threshold:
+ self.waypoint_index += 1
+
+ if self.waypoint_index >= len(self.waypoints):
+ self.done_time = t
+
+ premode = self.mode
+
+ if self.waypoint_index >= len(self.waypoints):
+ self.mode = 'Done'
+ else:
+ self.mode = f'Waypoint {self.waypoint_index + 1}'
+
+ rv = premode != self.mode
+
+ if rv:
+ self.log(f"Waypoint transition {premode} -> {self.mode} at time {t}")
+
+ return rv
+
+ def get_waypoint_data(self, x_f16):
+ '''returns current waypoint data tuple based on the current waypoint:
+
+ (heading, inclination, horiz_range, vert_range, slant_range)
+
+ heading = heading to tgt, equivalent to psi (rad)
+ inclination = polar angle to tgt, equivalent to theta (rad)
+ horiz_range = horizontal range to tgt (ft)
+ vert_range = vertical range to tgt (ft)
+ slant_range = total range to tgt (ft)
+ '''
+
+ waypoint = self.waypoints[self.waypoint_index]
+
+ e_pos = x_f16[StateIndex.POSE]
+ n_pos = x_f16[StateIndex.POSN]
+ alt = x_f16[StateIndex.ALT]
+
+ delta = [waypoint[i] - [e_pos, n_pos, alt][i] for i in range(3)]
+
+ _, inclination, slant_range = cart2sph(delta)
+
+ heading = wrap_to_pi(pi/2 - atan2(delta[1], delta[0]))
+
+ horiz_range = np.linalg.norm(delta[0:2])
+ vert_range = np.linalg.norm(delta[2])
+
+ return heading, inclination, horiz_range, vert_range, slant_range
+
+def get_nz_for_level_turn_ol(x_f16):
+ 'get nz to do a level turn'
+
+ # Pull g's to maintain altitude during bank based on trig
+
+ # Calculate theta
+ phi = x_f16[StateIndex.PHI]
+
+ if abs(phi): # if cos(phi) ~= 0, basically
+ nz = 1 / cos(phi) - 1 # Keeps plane at altitude
+ else:
+ nz = 0
+
+ return nz
+
+def get_path_angle(x_f16):
+ 'get the path angle gamma'
+
+ alpha = x_f16[StateIndex.ALPHA] # AoA (rad)
+ beta = x_f16[StateIndex.BETA] # Sideslip (rad)
+ phi = x_f16[StateIndex.PHI] # Roll anle (rad)
+ theta = x_f16[StateIndex.THETA] # Pitch angle (rad)
+
+ gamma = asin((cos(alpha)*sin(theta)- \
+ sin(alpha)*cos(theta)*cos(phi))*cos(beta) - \
+ (cos(theta)*sin(phi))*sin(beta))
+
+ return gamma
+
+def wrap_to_pi(psi_rad):
+ '''handle angle wrapping
+
+ returns equivelent angle in range [-pi, pi]
+ '''
+
+ rv = psi_rad % (2 * pi)
+
+ if rv > pi:
+ rv -= 2 * pi
+
+ return rv
+
+def cart2sph(pt3d):
+ '''
+ Cartesian to spherical coordinates
+
+ returns az, elev, r
+ '''
+
+ x, y, z = pt3d
+
+ h = sqrt(x*x + y*y)
+ r = sqrt(h*h + z*z)
+
+ elev = atan2(z, h)
+ az = atan2(y, x)
+
+ return az, elev, r
+
+if __name__ == '__main__':
+ print("Autopulot script not meant to be run directly.")
diff --git a/demo/ball_bounces.py b/demo/ball/ball_bounces.py
similarity index 100%
rename from demo/ball_bounces.py
rename to demo/ball/ball_bounces.py