Feature/traj generation

examples/uav_dynamics_model.py

@@ -0,0 +1,113 @@
+# Copyright © 2021 United States Government as represented by the Administrator of the
+# National Aeronautics and Space Administration.  All Rights Reserved.
+
+"""
+Example of generating a trajectory for a rotorcraft through a set of coarse waypoints 
+"""
+import numpy as np
+import matplotlib.pyplot as plt
+
+from prog_models.models.uav_model import UAVGen 
+
+def run_example(): 
+
+    # Example 1: generate trajectory from waypoints and ETAs 
+
+    # Define coarse waypoints: waypoints must be defined with a dictionary of numpy arrays or as columns in a text file 
+    # See documentation for specific information on inputting waypoints 
+    waypoints = {}
+    waypoints['lat_deg']   = np.array([37.09776, 37.09776, 37.09776, 37.09798, 37.09748, 37.09665, 37.09703, 37.09719, 37.09719, 37.09719, 37.09719, 37.09748, 37.09798, 37.09776, 37.09776])
+    waypoints['lon_deg']   = np.array([-76.38631, -76.38629, -76.38629, -76.38589, -76.3848, -76.38569, -76.38658, -76.38628, -76.38628, -76.38628, -76.38628, -76.3848, -76.38589, -76.38629, -76.38629])
+    waypoints['alt_ft']    = np.array([-1.9682394, 164.01995, 164.01995, 164.01995, 164.01995, 164.01995, 164.01995, 164.01995, 0.0, 0.0, 164.01995, 164.01995, 164.01995, 164.01995, 0.0])
+    waypoints['time_unix'] = np.array([1544188336, 1544188358, 1544188360, 1544188377, 1544188394, 1544188411, 1544188428, 1544188496, 1544188539, 1544188584, 1544188601, 1544188635, 1544188652, 1544188672, 1544188692])
+
+    # Define model parameters:
+    params_1 = {
+        'flight_plan': waypoints, # Specify waypoints 
+        'dt': 0.1, # Define time step for model generation; this value is also used for simulate_to functionality 
+        'vehicle_model': 'tarot18', # Define vehicle
+        'process_noise': 0
+    }
+
+    # Create a model object, define noise
+    uav_1 = UAVGen(**params_1)
+    uav_1.initialize()
+
+    # Set simulation options 
+    options = {
+        # 'dt': 0.1, # Note that this value isn't used internally, as simulate_to dt is defined as params_1['dt']; if dt is defined here as any other value, a warning will be returned
+        'save_freq': params_1['dt']
+    }
+
+    # Generate trajectory
+    traj_results_1 = uav_1.simulate_to_threshold(**options)
+
+    # Visualize results:
+    # Plot reference trajectory and generated trajectory - use 'visualize_traj' function in the UAVGen class
+    uav_1.visualize_traj(traj_results_1)
+
+    # Plot internal states 
+    traj_results_1.outputs.plot(keys = ['x', 'y', 'z'], ylabel = 'Cartesian position', title='Example 1 Predicted Outputs')
+    traj_results_1.outputs.plot(keys = ['phi', 'theta', 'psi'], ylabel = 'Pitch, roll, and yaw', title='Example 1 Predicted Outputs')
+    traj_results_1.outputs.plot(keys = ['vx', 'vy', 'vz'], ylabel = 'Velocities', title='Example 1 Predicted Outputs')
+    traj_results_1.outputs.plot(keys = ['p', 'q', 'r'], ylabel = 'Angular velocities',title='Example 1 Predicted Outputs')
+
+
+    # Example 2: generate trajectory from waypoints and speeds 
+    # For this example, we define waypoints without ETAs, and instead add speed specifications 
+
+    # Use same waypoints, but delete the ETAs
+    del waypoints['time_unix']
+
+    # Define model parameters - this time we must specify speeds since ETAs are not given as input
+    params_2 = {
+        'flight_plan': waypoints, # Specify waypoints 
+        'dt': 0.1, 
+        'vehicle_model': 'tarot18',
+        'cruise_speed': 6.0, # Additionally specify speeds 
+        'ascent_speed': 3.0,
+        'descent_speed': 3.0, 
+        'landing_speed': 1.5,
+        'process_noise': 0
+    }
+
+    # Create a model object, define noise
+    uav_2 = UAVGen(**params_2)
+
+    # Generate trajectory
+    traj_results_2 = uav_2.simulate_to_threshold(**options)
+
+    # Visualize results:
+    # Plot reference trajectory and generated trajectory
+    uav_2.visualize_traj(traj_results_2)
+
+
+    # Example 3: generate trajectory using complex UAV model parameters and waypoint information from text file
+    # The UAV model class has other optional parameters for more specific modeling. Here, we illustrate the use of a few of these parameters.
+
+    # Define model parameters - this time we must specify speeds since ETAs are not given as input
+    params_3 = {
+        'flight_file': 'examples/uav_waypoints.txt', # Specify path of text file with waypoints 
+        'dt': 0.1, 
+        'vehicle_model': 'tarot18',
+        'payload': 5.0, # kg, Add payload to vehicle
+        'hovering_time': 10.0, # s, Add hovering time between each waypoint
+        'final_time_buffer_sec': 15, # s, Defines acceptable time interval to reach final waypoint 
+        'process_noise': 0
+    }
+
+    # Create a model object, define noise
+    uav_3 = UAVGen(**params_3)
+
+    # Generate trajectory
+    traj_results_3 = uav_3.simulate_to_threshold(**options)
+
+    # Visualize results:
+    # Plot reference trajectory and generated trajectory
+    uav_3.visualize_traj(traj_results_3)
+    plt.show()
+
+# This allows the module to be executed directly 
+if __name__ == '__main__':
+    run_example()
+

examples/uav_waypoints.txt

@@ -0,0 +1,16 @@
+lat_deg	lon_deg	alt_ft	time_unix
+37.09776	-76.38631	-1.9682394	1544188336
+37.09776	-76.38629	164.01995	1544188358
+37.09776	-76.38629	164.01995	1544188360
+37.09798	-76.38589	164.01995	1544188377
+37.09748	-76.3848	164.01995	1544188394
+37.09665	-76.38569	164.01995	1544188411
+37.09703	-76.38658	164.01995	1544188428
+37.09719	-76.38628	164.01995	1544188496
+37.09719	-76.38628	0.0		1544188539
+37.09719	-76.38628	0.0		1544188584
+37.09719	-76.38628	164.01995	1544188601
+37.09748	-76.3848	164.01995	1544188635
+37.09798	-76.38589	164.01995	1544188652
+37.09776	-76.38629	164.01995	1544188672
+37.09776	-76.38629	0.0		1544188692

src/prog_models/models/uav_model/__init__.py

@@ -0,0 +1,4 @@
+# Copyright © 2021 United States Government as represented by the Administrator of the
+# National Aeronautics and Space Administration.  All Rights Reserved.
+
+from .uav_model import UAVGen

src/prog_models/models/uav_model/trajectory/load_trajectories.py

@@ -0,0 +1,88 @@
+# Copyright © 2021 United States Government as represented by the Administrator of the
+# National Aeronautics and Space Administration.  All Rights Reserved.
+
+"""
+Functions to extract user-defined waypoint information and convert to appropriate units for trajectory generation
+"""
+# Imports 
+import numpy as np
+import datetime as dt
+
+from ..utilities import loadsavewrite_utils as loadsave
+from prog_models.exceptions import ProgModelInputException
+
+DEG2RAD = np.pi/180.0
+FEET2MET = 0.3048
+
+def get_flightplan(fname, **kwargs):
+    """
+    Flight plan is input by user in text file format. 
+    Flight plan in .txt file must be organized as follows (with the following column headers):
+    columns: latitude (lat_deg or lat_rad), longitude (lon_deg or lon_rad), altitude (alt_ft or alt_m), and (optional) time (time_unix)
+    rows:   header first row, data from second row onward
+
+    :param fname:         str, file path including file name and txt extension
+    :param  kwargs:       options for numpy loadtxt function: skiprows (default=1), comments (default #), max_rows (default None)
+    :return:              flight plan dictionary with keys: lat (in rad), lon (in rad), alt (in m), time_unix, time_stamp, name (i.e., filename).
+    """
+
+    params = dict(skiprows=1, comments='#', max_rows=None)
+    params.update(kwargs)
+
+    if fname[fname.rfind('.')+1:] == 'txt':
+        lat, lon, alt, time_unix, timestamps = loadsave.load_traj_from_txt(fname, params['skiprows'], params['comments'], params['max_rows'])
+    elif fname[fname.rfind('.')+1:] == 'mat':
+        lat, lon, alt, time_unix, timestamps = loadsave.load_traj_from_mat_file(fname)
+
+    # If no time stamp was available from file, add current time stamp and corresponding unix time.
+    if timestamps is None or time_unix is None:
+        timestamps = [dt.datetime.now()]
+        time_unix  = [timestamps[0].timestamp()]
+
+    flightplan_ = {'lat': lat, 'lon': lon, 'alt': alt, 'time_unix': time_unix, 'timestamp': timestamps, 'name': fname}
+    return flightplan_
+
+def convert_dict_inputs(input_dict):
+    """
+    Flight plan is input by user in dictionary format. 
+    Dictionary must contain keys for: latitude ('lat_deg' or 'lat_rad'), longitude ('lon_deg' or 'lon_rad'), altitude ('alt_ft' or 'alt_m'), and (optional) time ('time_unix')
+    Each dictionary key must have a corresponding numpy array of the appropriate values 
+
+    :param input_dict:    dictionary with waypoints latitude, longitude, altitude, and optional time defined as numpy arrays        
+    :return:              flight plan dictionary with keys: lat (in rad), lon (in rad), alt (in m), time_unix, time_stamp.
+    """
+
+    # Check units and return warnings if incorrect:
+    if 'lat_deg' not in input_dict.keys() and 'lat_rad' not in input_dict.keys():
+        raise ProgModelInputException("Waypoints latitude must be defined in degrees (with lat_deg) or radians (with lat_rad).")
+    elif 'lon_deg' not in input_dict.keys() and 'lon_rad' not in input_dict.keys():
+        raise ProgModelInputException("Waypoints longitude must be defined in degrees (with lon_deg) or radians (with lon_rad).")
+    elif 'alt_ft' not in input_dict.keys() and 'alt_m' not in input_dict.keys():
+        raise ProgModelInputException("Waypoints altitude must be defined in feet (with alt_ft) or meters (with alt_m).")
+    if len(input_dict.keys()) > 3 and 'time_unix' not in input_dict.keys():
+        raise ProgModelInputException("Waypoints input incorrectly. Use lat_deg, lon_deg, alt_ft, and time_unix to specify.")
+    input_shape = [input_dict[key].shape for key in input_dict.keys()]
+    if all(input_shape[iter] == input_shape[0] for iter in range(len(input_shape))) == False:
+        raise ProgModelInputException("Waypoints input incorectly. Arrays of lat/lon/alt/time have different dimensions.")
+
+    # Convert, if necessary
+    if 'lat_deg' in input_dict.keys():
+        lat = input_dict['lat_deg'] * DEG2RAD
+        lon = input_dict['lon_deg'] * DEG2RAD
+    else: 
+        lat = input_dict['lat_rad']
+        lon = input_dict['lon_rad']
+    if 'alt_ft' in input_dict.keys():
+        alt = input_dict['alt_ft'] * FEET2MET
+    else: 
+        alt = input_dict['alt_m']
+
+    if 'time_unix' in input_dict.keys():
+        time_unix = input_dict['time_unix']
+        timestamps = [dt.datetime.fromtimestamp(time_unix[ii]) for ii in range(len(time_unix))]
+    else: 
+        # If no time stamp was available from file, add current time stamp and corresponding unix time.
+        timestamps = [dt.datetime.now()]
+        time_unix  = [timestamps[0].timestamp()]
+
+    return {'lat_rad': lat, 'lon_rad': lon, 'alt_m': alt, 'timestamp': timestamps, 'time_unix': time_unix}