Feature/traj generation #480
Conversation
Included: changes to match Matteo's newest code, changes to what is required from user input and warnings/exceptions if wrong input given, checking if speeds required for input ETAs are beyond limits, revert to dx implementation with vehicle state calculation, getting rid of integrator_fn parameter, added PD controller
Added computations for event_state and threshold_met, also deleted previous integrator parameter (already built in to ProgPy)
Added capability for user to input data of different units, and checks for appropriate data. Cleaned up assert/exceptions.
|
Thank you for opening this PR. Each PR into dev requires a code review. For the code review, look at the following:
|
There was a problem hiding this comment.
Submitting partial review. I'm still working on it.
Great work! It looks really great and I'm so excited that you have it working. I made a few suggestions. Let me know if you have any questions. Otherwise, I'll continue reviewing tomorrow.
Have a great long weekend!
| from warnings import warn | ||
| from prog_models.exceptions import ProgModelInputException | ||
|
|
||
| class UAVGen(PrognosticsModel): |
There was a problem hiding this comment.
Is UAVGen the clearest name for this? What about just UAV or UAVFlight (since it's a model of its flight)
There was a problem hiding this comment.
That's a good question and we've been talking about it.
Probably UAVFlight would be the best in this situation. However, we've also talked about increasing the ``modularity'' of this model, but generating a controller that then is assigned to the UAV model. Also, trajectory generation is independent of the UAV model (up to minor things like: what type of UAV it is (rotorcraft vs. fixed wind vs. something else, maximum speed/acceleration possible, and stuff like that).
So maybe we should re-factor this to make it indeed modular? don't know if we want to leave it for the future.
| wypt_z.append(z_temp) | ||
| self.parameters['waypoints'] = {'waypoints_time': wypt_time_unix, 'waypoints_x': wypt_x, 'waypoints_y': wypt_y, 'waypoints_z': wypt_z, 'next_waypoint': 0} | ||
|
|
||
| # Generate trajectory |
There was a problem hiding this comment.
How is a trajectory different from waypoints? Additional detail? Could use a comment here for people not familiar
| def event_state(self, x : dict) -> dict: | ||
| # Extract next waypoint information | ||
| num_wypts = len(self.parameters['waypoints']['waypoints_time']) - 1 # Don't include initial waypoint | ||
| index_next = self.parameters['waypoints']['next_waypoint'] |
There was a problem hiding this comment.
The model itself should be stateless. Some algorithms might require that you run multiple states through the model (e.g., when building a surrogate model). This will break down if you save state in the parameters. In this case, the next_waypoint is part of the state of the system.
next_waypoint should be part of the state vector and updated in next_state, or it should be calculated each time.
Since this is a bigger change, I'm ok with leaving it as is (with a note about this limitation) if you add this to the github issue for the next release.
There was a problem hiding this comment.
@teubert I'd love to talk with you more about this. For now, I've added it to the future work issue but hopefully we can get it done before the PR is finalized
| } | ||
| else: | ||
| # Trajectory has passed acceptable bounds of final ETA - simulation terminated | ||
| warn("Trajectory simulation extends beyond the final ETA. Either the final waypoint was not reached in the alotted time (and the simulation was terminated), or simulation was run for longer than the trajectory length.") |
There was a problem hiding this comment.
What if the final waypoint hasn't been met yet, but we're running behind? Is that a case you would want to support
There was a problem hiding this comment.
It will continue simulating for a buffer time defined by the parameter "final_time_buffer_sec" past the final ETA time. Originally I implemented it this way because I was thinking that if the final waypoint wasn't met, the simulation would go on forever. But now I'm realizing that the default horizon would eventually stop this. However, once we pass the final ETA, we won't have any new input information, so the results quickly level out to whatever output corresponds to that input. Does that make sense? I'm happy to leave it as is or get rid of this and let it run to horizon
There was a problem hiding this comment.
OK- I see why you implemented it that way it is. If you're simulating to threshold wouldn't it stop anyways at the last waypoint, so it wouldn't simulate past the last input information.
I guess that doesn't help for simulate to. I'm hoping you can remove this when we move to the modular, since input would be provided by the future loading equation instead. I don't like the idea of having two horizons
|
I noticed that, generally in prognostic_model, the method simulate_to_threshold initializes u before initializing x, such that when future_loading_eqn is called the first time x=None. I'm having a deja-vu (I believe we already talked about it but not sure what the problem would be). |
Editing imports for readability, other readability changes, adjusting outputs, adding __init__
…e lat-lon-alt in one shot instead of for loop. output container generated with a for loop instead of explicitly calling all keys. However, this might need to be changed in the future when only a subset of the state will become an output. future_loading_new compressed. there's still the issue of having vehicle_model.state AND x as both states.
| if params['R'] is None: params['R'] = np.diag([500, 4000, 4000, 4000]) # T, Mu, Mv, Mw | ||
| if params['qi'] is None: params['qi'] = np.array([100, 100, 1000]) # Integral error weights in position x, y, z | ||
| if params['i_lag'] is None: params['i_lag'] = 100 # integral lag: how far back to use (in data points, so 1 point = 1 dt) for integral error | ||
|
|
There was a problem hiding this comment.
here I cleaned up some controller parameter's definition which have been there for a while (my fault). The trajectories now look a little cleaner as well.
| coord_end = geom.Coord(route_.lat[0], route_.lon[0], route_.alt[0]) | ||
| self.coord_transform = deepcopy(coord_end) | ||
| wypt_time_unix = [datetime.datetime.timestamp(route_.eta[iter]) - datetime.datetime.timestamp(route_.eta[0]) for iter in range(len(route_.eta))] | ||
| wypt_x, wypt_y, wypt_z = coord_end.geodetic2enu(route_.lat, route_.lon, route_.alt) # computing way-points in ENU in one shot |
There was a problem hiding this comment.
here there was a for loop I substituted (functions in geom.Coord work with vectors).
| self.vehicle_model.set_state(state=state_temp + dxdt*self.parameters['dt']) | ||
|
|
||
| # I'd suggest a more compact way of generating the StateContainer | ||
| return self.StateContainer(np.array([np.atleast_1d(item) for item in dxdt])) |
There was a problem hiding this comment.
in-line for loop to generated state container. it's just more compact.
| .. [0] M. Corbetta et al., "Real-time UAV trajectory prediction for safely monitoring in low-altitude airspace," AIAA Aviation 2019 Forum, 2019. https://arc.aiaa.org/doi/pdf/10.2514/6.2019-3514 | ||
| """ | ||
| events = ['TrajectoryComplete'] | ||
| inputs = ['T','mx','my','mz'] |
There was a problem hiding this comment.
| inputs = ['T','mx','my','mz'] | |
| inputs = [] |
Since 'T','mx','my','mz' are calculated by the controller, which is internal to the model, I think these should not be listed as inputs. Do I understand that correctly?
There was a problem hiding this comment.
I think this is an issue with our current implementation, where the "inputs" are defined in the internal "future_loading" function. They still act as inputs in the normal sense of u in the overall ProgPy framework, even though they aren't user defined. If I change the code as suggested here, I get an error, since u is still used throughout (for example, in the dx calculation on line 309). The more we review, the more I think it's time to switch to a more modular version ...
|
|
||
| import geometry as geom | ||
| from .nurbs import generate_3dnurbs, generate_intermediate_points, evaluate, knot_vector | ||
| import prog_models.models.uav_model.trajectory.load_trajectories as load |
There was a problem hiding this comment.
| import prog_models.models.uav_model.trajectory.load_trajectories as load | |
| import .load_trajectories as load |
Imports clean up, deleting unnecessary pass, clean up example
|
Benchmarking Results [Update]
To:
|
This model describes the dynamics of a small UAV and generates predictions of a flyable trajectory given a set of waypoints.
This work was previously developed by @matteocorbetta and @portia19, and model development is described in Corbetta et al. 2019, "Real-time UAV trajectory prediction for safely monitoring in low-altitude airspace".
PR includes all necessary functionality for:
Future work described here: nasa/progpy#49