Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

Printing Coordinates and degrees as input #140

Merged
merged 4 commits into from
Dec 21, 2023
Merged

Printing Coordinates and degrees as input #140

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
4 commits
Select commit Hold shift + click to select a range
8845332
Adds string describing coordinate point when printed
teubert Dec 20, 2023
56366e4
fixed unit
teubert Dec 20, 2023
b99553c
Change input to degrees
teubert Dec 20, 2023
aee4ff3
Update docs
teubert Dec 20, 2023
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
8 changes: 4 additions & 4 deletions examples/uav_dynamics_model.py
View file
Open in desktop
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
Open in desktop
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
Open in desktop
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
Open in desktop
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
Loading