diff --git a/examples/uav_dynamics_model.py b/examples/uav_dynamics_model.py index 8109242..3b7fea9 100644 --- a/examples/uav_dynamics_model.py +++ b/examples/uav_dynamics_model.py @@ -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) @@ -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, diff --git a/src/progpy/utils/traj_gen/geometry.py b/src/progpy/utils/traj_gen/geometry.py index 0c7ec14..cd1604b 100644 --- a/src/progpy/utils/traj_gen/geometry.py +++ b/src/progpy/utils/traj_gen/geometry.py @@ -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): """ @@ -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): """ diff --git a/src/progpy/utils/traj_gen/trajectory.py b/src/progpy/utils/traj_gen/trajectory.py index 6800ba9..c5efae0 100644 --- a/src/progpy/utils/traj_gen/trajectory.py +++ b/src/progpy/utils/traj_gen/trajectory.py @@ -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): """ @@ -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): @@ -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, @@ -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): @@ -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, diff --git a/tests/test_uav_model.py b/tests/test_uav_model.py index 5c4fd76..087958e 100644 --- a/tests/test_uav_model.py +++ b/tests/test_uav_model.py @@ -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]] @@ -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 @@ -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']