Skip to content

Commit

Permalink
Merge branch 'dev' into feature/138-composite-model-function-output
Browse files Browse the repository at this point in the history
  • Loading branch information
@teubert
teubert authored Dec 21, 2023
2 parents 7d6a96a + ec48258 commit a367143
Show file tree
Hide file tree
Showing 4 changed files with 51 additions and 46 deletions.
8 changes: 4 additions & 4 deletions examples/uav_dynamics_model.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -41,8 +41,8 @@ def run_example():
# Generate trajectory
# =====================
# Generate trajectory object and pass the route (waypoints, ETA) to it
traj = Trajectory(lat=lat_deg * np.pi/180.0,
lon=lon_deg * np.pi/180.0,
traj = Trajectory(lat=lat_deg,
lon=lon_deg,
alt=alt_ft * 0.3048,
etas=time_unix)

Expand Down Expand Up @@ -74,8 +74,8 @@ def run_example():

# Generate trajectory object and pass the route (lat/lon/alt, no ETAs)
# and speed information to it
traj_speed = Trajectory(lat=lat_deg * np.pi/180.0,
lon=lon_deg * np.pi/180.0,
traj_speed = Trajectory(lat=lat_deg,
lon=lon_deg,
alt=alt_ft * 0.3048,
cruise_speed=8.0,
ascent_speed=2.0,
Expand Down
16 changes: 11 additions & 5 deletions src/progpy/utils/traj_gen/geometry.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -7,6 +7,9 @@

import numpy as np

RAD2DEG: float = (180.0/np.pi)
DEG2RAD: float = (np.pi/180.0)

# EARTH-RELATED DISTANCE FUNCTIONS
def greatcircle_distance(lat1, lat2, lon1, lon2, R=6371e3):
"""
Expand Down Expand Up @@ -348,18 +351,21 @@ def __init__(self, lat0, lon0, alt0):
"""
Initialize Coordinate frame class.
:param lat0: Latitude of origin of reference frame
:param lon0: Longitude of origin of reference frame
:param alt0: Altitude of origin of reference frame
:param lat0: Latitude of origin of reference frame, deg
:param lon0: Longitude of origin of reference frame, deg
:param alt0: Altitude of origin of reference frame, m
"""
self.a = 6378137.0 # [m], equatorial radius
self.f = 1.0 / 298.257223563 # [-], ellipsoid flatness
self.b = self.a * (1.0 - self.f) # [m], polar radius
self.e = np.sqrt(self.f * (2 - self.f)) # [-], eccentricity
self.lat0 = lat0
self.lon0 = lon0
self.lat0 = lat0 * DEG2RAD
self.lon0 = lon0 * DEG2RAD
self.alt0 = alt0
self.N0 = self.a / np.sqrt(1 - self.e**2.0 * np.sin(self.lat0)**2.0) # [m], Radius of curvature on the Earth

def __str__(self):
return f'coordinate at {round(np.abs(self.lat0*RAD2DEG), 4)}{"°N" if (np.sign(self.lat0) >= 0) else "°S"} {round(np.abs(self.lon0*RAD2DEG), 4)}{"°E" if (np.sign(self.lon0) >= 0) else "°W"} and {self.alt0}m elevation'

def ecef2enu(self, xecef, yecef, zecef):
"""
Expand Down
22 changes: 12 additions & 10 deletions src/progpy/utils/traj_gen/trajectory.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -13,6 +13,8 @@
from progpy.utils.traj_gen import geometry as geom
from progpy.utils.traj_gen.nurbs import NURBS

DEG2RAD: float = (np.pi/180.0)


class TrajectoryFigure(Figure):
"""
Expand Down Expand Up @@ -165,11 +167,11 @@ class Trajectory():
args:
lat (np.ndarray):
rad, n x 1 array, doubles, latitude coordinates of waypoints
rad, n x 1 array, doubles, latitude coordinates of waypoints, deg
lon (np.ndarray):
rad, n x 1 array, doubles, longitude coordinates of waypoints
rad, n x 1 array, doubles, longitude coordinates of waypoints, deg
alt (np.ndarray):
m, n x 1 array, doubles, altitude coordinates of waypoints
m, n x 1 array, doubles, altitude coordinates of waypoints, m
takeoff_time (float, optional):
take off time of the trajectory. Default is None (starting at current time).
etas (list[float], optional):
Expand Down Expand Up @@ -234,8 +236,8 @@ def __init__(self,

self.trajectory = {}
self.waypoints = {
'lat': lat,
'lon': lon,
'lat': [elem*DEG2RAD for elem in lat],
'lon': [elem*DEG2RAD for elem in lon],
'alt': alt,
'takeoff_time': takeoff_time,
'eta': etas,
Expand All @@ -259,9 +261,9 @@ def __init__(self,
# Set up coordinate system conversion between Geodetic,
# Earth-Centric Earth-Fixed (ECF), and Cartesian (East-North-Up, ENU)
self.coordinate_system = geom.Coord(
self.waypoints['lat'][0],
self.waypoints['lon'][0],
self.waypoints['alt'][0])
lat[0],
lon[0],
alt[0])

# Define speed parameters - only necessary if ETAs are not defined
if etas is not None and ('cruise_speed' in kwargs or 'ascent_speed' in kwargs or 'descent_speed' in kwargs or 'landing_speed' in kwargs):
Expand Down Expand Up @@ -292,8 +294,8 @@ def __init__(self,

# Interpolation properties
self.parameters = {'gravity': 9.81,
'max_phi': 45/180.0*np.pi,
'max_theta': 45/180.0*np.pi,
'max_phi': 0.25*np.pi,
'max_theta': 0.25*np.pi,
'max_iter': 10,
'max_avgjerk': 20.0,
'nurbs_order': 4,
Expand Down
51 changes: 24 additions & 27 deletions tests/test_uav_model.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -36,15 +36,13 @@ def test_reference_trajectory_generation(self):
waypoints_dict['alt_ft'] = np.array([-1.9682394, 164.01995, 164.01995, 164.01995, 164.01995, 164.01995, 164.01995, 164.01995, 0.0])
waypoints_dict['time_unix'] = [1544188336, 1544188358, 1544188360, 1544188377, 1544188394, 1544188411, 1544188428, 1544188496, 1544188539]

lat_in = waypoints_dict['lat_deg'] * np.pi/180.0
lon_in = waypoints_dict['lon_deg'] * np.pi/180.0
alt_in = waypoints_dict['alt_ft'] * 0.3048
etas_in = waypoints_dict['time_unix']
takeoff_time = etas_in[0]
etas_wrong = [dt.datetime.fromtimestamp(waypoints_dict['time_unix'][ii]) for ii in range(len(waypoints_dict['time_unix']))]

lat_small = np.array([lat_in[0]])
lon_small = np.array([lon_in[0]])
lat_small = np.array([waypoints_dict['lat_deg'][0]])
lon_small = np.array([waypoints_dict['lon_deg'][0]])
alt_small = np.array([alt_in[0]])
etas_small = [etas_in[0]]

Expand All @@ -54,73 +52,72 @@ def test_reference_trajectory_generation(self):
ref_traj = Trajectory()
with self.assertRaises(TypeError):
# Only subset of waypoint information provided
ref_traj = Trajectory(lon=lon_in, alt=alt_in, takeoff_time=takeoff_time, etas=etas_in)
ref_traj = Trajectory(lon=waypoints_dict['lon_deg'], alt=alt_in, takeoff_time=takeoff_time, etas=etas_in)
with self.assertRaises(TypeError):
# Only subset of waypoint information provided
ref_traj = Trajectory(lat=lat_in, alt=alt_in, takeoff_time=takeoff_time, etas=etas_in)
ref_traj = Trajectory(lat=waypoints_dict['lat_deg'], alt=alt_in, takeoff_time=takeoff_time, etas=etas_in)
with self.assertRaises(TypeError):
# Only subset of waypoint information provided
ref_traj = Trajectory(lat=lat_in, lon=lon_in, takeoff_time=takeoff_time, etas=etas_in)
ref_traj = Trajectory(lat=waypoints_dict['lat_deg'], lon=waypoints_dict['lon_deg'], takeoff_time=takeoff_time, etas=etas_in)

# Waypoints defined incorrectly
# Wrong type for waypoints
with self.assertRaises(TypeError):
# Waypoints defined incorrectly; must be numpy arrays
ref_traj = Trajectory(lat='a', lon=lon_in, alt=alt_in, takeoff_time=takeoff_time, etas=etas_in)
ref_traj = Trajectory(lat='a', lon=waypoints_dict['lon_deg'], alt=alt_in, takeoff_time=takeoff_time, etas=etas_in)
with self.assertRaises(TypeError):
# Waypoints defined incorrectly; must be numpy arrays
ref_traj = Trajectory(lat=lat_in, lon=[1, 2, 3], alt=alt_in, takeoff_time=takeoff_time, etas=etas_in)
ref_traj = Trajectory(lat=waypoints_dict['lat_deg'], lon=[1, 2, 3], alt=alt_in, takeoff_time=takeoff_time, etas=etas_in)
with self.assertRaises(TypeError):
# Waypoints defined incorrectly; must be numpy arrays
ref_traj = Trajectory(lat=lat_in, lon=lon_in, alt=1, takeoff_time=takeoff_time, etas=etas_in)
ref_traj = Trajectory(lat=waypoints_dict['lat_deg'], lon=waypoints_dict['lon_deg'], alt=1, takeoff_time=takeoff_time, etas=etas_in)
with self.assertRaises(TypeError):
# Waypoints defined incorrectly; must be numpy arrays
ref_traj = Trajectory(lat={'lat': 1}, lon=lon_in, alt=alt_in, takeoff_time=takeoff_time, etas=etas_in)
ref_traj = Trajectory(lat={'lat': 1}, lon=waypoints_dict['lon_deg'], alt=alt_in, takeoff_time=takeoff_time, etas=etas_in)
with self.assertRaises(TypeError):
# Waypoints defined incorrectly; takeoff_time must be float/int
ref_traj = Trajectory(lat=lat_in, lon=lon_in, alt=alt_in, takeoff_time=np.array([1]), etas=etas_in)
ref_traj = Trajectory(lat=waypoints_dict['lat_deg'], lon=waypoints_dict['lon_deg'], alt=alt_in, takeoff_time=np.array([1]), etas=etas_in)
with self.assertRaises(TypeError):
# Waypoints defined incorrectly; takeoff_time must be float/int
ref_traj = Trajectory(lat=lat_in, lon=lon_in, alt=alt_in, takeoff_time='abc', etas=etas_in)
ref_traj = Trajectory(lat=waypoints_dict['lat_deg'], lon=waypoints_dict['lon_deg'], alt=alt_in, takeoff_time='abc', etas=etas_in)
with self.assertRaises(TypeError):
# Waypoints defined incorrectly; etas must be list of float/int
ref_traj = Trajectory(lat=lat_in, lon=lon_in, alt=alt_in, takeoff_time=takeoff_time, etas=etas_wrong)
ref_traj = Trajectory(lat=waypoints_dict['lat_deg'], lon=waypoints_dict['lon_deg'], alt=alt_in, takeoff_time=takeoff_time, etas=etas_wrong)
with self.assertRaises(TypeError):
# Waypoints defined incorrectly; etas must be list of float/int
ref_traj = Trajectory(lat=lat_in, lon=lon_in, alt=alt_in, takeoff_time=takeoff_time, etas=np.array([1, 2, 3]))
ref_traj = Trajectory(lat=waypoints_dict['lat_deg'], lon=waypoints_dict['lon_deg'], alt=alt_in, takeoff_time=takeoff_time, etas=np.array([1, 2, 3]))

# Wrong lengths for waypoints
with self.assertRaises(ValueError):
ref_traj = Trajectory(lat=lat_in[:5], lon=lon_in, alt=alt_in, takeoff_time=takeoff_time, etas=etas_in)
ref_traj = Trajectory(lat=waypoints_dict['lat_deg'][:5], lon=waypoints_dict['lon_deg'], alt=alt_in, takeoff_time=takeoff_time, etas=etas_in)
with self.assertRaises(ValueError):
ref_traj = Trajectory(lat=lat_in, lon=lon_in[2:4], alt=alt_in, takeoff_time=takeoff_time, etas=etas_in)
ref_traj = Trajectory(lat=waypoints_dict['lat_deg'], lon=waypoints_dict['lon_deg'][2:4], alt=alt_in, takeoff_time=takeoff_time, etas=etas_in)
with self.assertRaises(ValueError):
ref_traj = Trajectory(lat=lat_in, lon=lon_in, alt=alt_in[1:7], takeoff_time=takeoff_time, etas=etas_in)
ref_traj = Trajectory(lat=waypoints_dict['lat_deg'], lon=waypoints_dict['lon_deg'], alt=alt_in[1:7], takeoff_time=takeoff_time, etas=etas_in)
with self.assertRaises(ValueError):
ref_traj = Trajectory(lat=lat_in, lon=lon_in, alt=alt_in, takeoff_time=takeoff_time, etas=etas_in[:5])
ref_traj = Trajectory(lat=waypoints_dict['lat_deg'], lon=waypoints_dict['lon_deg'], alt=alt_in, takeoff_time=takeoff_time, etas=etas_in[:5])
with self.assertRaises(ValueError):
ref_traj = Trajectory(lat=lat_small, lon=lon_small, alt=alt_small, etas=etas_small, takeoff=takeoff_time)

# Checking correct combination of ETAs and speeds
with self.assertRaises(UserWarning):
# No ETAs or speeds provided, warning is thrown
ref_traj = Trajectory(lat=lat_in, lon=lon_in, alt=alt_in, takeoff_time=takeoff_time)
ref_traj = Trajectory(lat=waypoints_dict['lat_deg'], lon=waypoints_dict['lon_deg'], alt=alt_in, takeoff_time=takeoff_time)
with self.assertRaises(UserWarning):
# Both ETAs and speeds provided, warning is thrown
params = {'cruise_speed': 1, 'descent_speed': 1, 'ascent_speed': 1, 'landing_speed': 1}
ref_traj = Trajectory(lat=lat_in, lon=lon_in, alt=alt_in, takeoff_time=takeoff_time, etas=etas_in, **params)
ref_traj = Trajectory(lat=waypoints_dict['lat_deg'], lon=waypoints_dict['lon_deg'], alt=alt_in, takeoff_time=takeoff_time, etas=etas_in, **params)

# Test trajectory generation functionality is generating an accurate result
# Convert waypoints to Cartesian
DEG2RAD = np.pi/180.0
FEET2MET = 0.3048
coord = geom.Coord(lat0=waypoints_dict['lat_deg'][0]*DEG2RAD, lon0=waypoints_dict['lon_deg'][0]*DEG2RAD, alt0=waypoints_dict['alt_ft'][0]*FEET2MET)
x_ref, y_ref, z_ref = coord.geodetic2enu(waypoints_dict['lat_deg']*DEG2RAD, waypoints_dict['lon_deg']*DEG2RAD, waypoints_dict['alt_ft']*FEET2MET)
coord = geom.Coord(lat0=waypoints_dict['lat_deg'][0], lon0=waypoints_dict['lon_deg'][0], alt0=waypoints_dict['alt_ft'][0]*FEET2MET)
x_ref, y_ref, z_ref = coord.geodetic2enu(waypoints_dict['lat_deg']*np.pi/180.0, waypoints_dict['lon_deg']*np.pi/180.0, waypoints_dict['alt_ft']*FEET2MET)
time_ref = [waypoints_dict['time_unix'][iter] - waypoints_dict['time_unix'][0] for iter in range(len(waypoints_dict['time_unix']))]

# Generate trajectory
vehicle.parameters['dt'] = 1
traj = Trajectory(lat=lat_in, lon=lon_in, alt=alt_in, etas=etas_in, takeoff_time=takeoff_time)
traj = Trajectory(lat=waypoints_dict['lat_deg'], lon=waypoints_dict['lon_deg'], alt=alt_in, etas=etas_in, takeoff_time=takeoff_time)
ref_traj_test = traj.generate(dt=vehicle.parameters['dt'])

# Check that generated trajectory is close to waypoints
Expand Down Expand Up @@ -152,8 +149,8 @@ def test_controllers_and_vehicle(self):
waypoints_dict['alt_ft'] = np.array([-1.9682394, 164.01995, 164.01995, 164.01995, 164.01995, 164.01995, 164.01995, 164.01995, 0.0])
waypoints_dict['time_unix'] = [1544188336, 1544188358, 1544188360, 1544188377, 1544188394, 1544188411, 1544188428, 1544188496, 1544188539]

lat_in = waypoints_dict['lat_deg'] * np.pi/180.0
lon_in = waypoints_dict['lon_deg'] * np.pi/180.0
lat_in = waypoints_dict['lat_deg']
lon_in = waypoints_dict['lon_deg']
alt_in = waypoints_dict['alt_ft'] * 0.3048
etas_in = waypoints_dict['time_unix']

Expand Down

0 comments on commit a367143

Please sign in to comment.