diff --git a/examples/basic_usage.py b/examples/basic_usage.py index 159e503..d72050b 100644 --- a/examples/basic_usage.py +++ b/examples/basic_usage.py @@ -97,6 +97,7 @@ results = sim_instance.run(t_final = 20, # The maximum duration of th plot_estimator = True, # Boolean: plots the estimator filter states and covariance diagonal elements plot_imu = True, # Boolean: plots the IMU measurements animate_bool = True, # Boolean: determines if the animation of vehicle state will play. + animate_wind = False, # Boolean: determines if the animation will include a scaled wind vector to indicate the local wind acting on the UAV. verbose = True, # Boolean: will print statistics regarding the simulation. fname = None # Filename is specified if you want to save the animation. The save location is rotorpy/data_out/. ) diff --git a/media/gusty.gif b/media/gusty.gif index 9b0cd5a..2bb010a 100644 Binary files a/media/gusty.gif and b/media/gusty.gif differ diff --git a/rotorpy/environments.py b/rotorpy/environments.py index 5611594..7c0d826 100644 --- a/rotorpy/environments.py +++ b/rotorpy/environments.py @@ -96,6 +96,7 @@ class Environment(): plot_estimator = True, # Boolean: plots the estimator filter states and covariance diagonal elements plot_imu = True, # Boolean: plots the IMU measurements animate_bool = False, # Boolean: determines if the animation of vehicle state will play. + animate_wind = False, # Boolean: determines if the animation will include a wind vector. verbose = False, # Boolean: will print statistics regarding the simulation. fname = None # Filename is specified if you want to save the animation. Default location is the home directory. ): @@ -137,7 +138,7 @@ class Environment(): visualizer = Plotter(self.result, self.world) if animate_bool: # Do animation here - visualizer.animate_results(fname=fname) + visualizer.animate_results(fname=fname, animate_wind=animate_wind) if plot: # Do plotting here visualizer.plot_results(plot_mocap=plot_mocap,plot_estimator=plot_estimator,plot_imu=plot_imu) diff --git a/rotorpy/utils/animate.py b/rotorpy/utils/animate.py index e5c2312..81b9cea 100644 --- a/rotorpy/utils/animate.py +++ b/rotorpy/utils/animate.py @@ -40,7 +40,7 @@ def _decimate_index(time, sample_time): sample_index = np.round(np.interp(sample_time, time, index)).astype(int) return sample_index -def animate(time, position, rotation, world, filename=None, blit=False, show_axes=True, close_on_finish=False): +def animate(time, position, rotation, wind, animate_wind, world, filename=None, blit=False, show_axes=True, close_on_finish=False): """ Animate a completed simulation result based on the time, position, and rotation history. The animation may be viewed live or saved to a .mp4 video @@ -54,6 +54,8 @@ def animate(time, position, rotation, world, filename=None, blit=False, show_axe time, (N,) with uniform intervals position, (N,3) rotation, (N,3,3) + wind, (N,3) world wind velocity + animate_wind, if True animate wind vector world, a World object filename, for saved video, or live view if None blit, if True use blit for faster animation, default is False @@ -65,6 +67,13 @@ def animate(time, position, rotation, world, filename=None, blit=False, show_axe rtf = 1.0 # real time factor > 1.0 is faster than real time playback render_fps = 30 + # Normalize the wind by the max of the wind magnitude on each axis, so that the maximum length of the arrow is decided by the scale factor + wind_mag = np.linalg.norm(wind, axis=1) # Get the wind magnitude time series + max_wind = np.max(wind_mag) # Find the maximum wind magnitude in the time series + if max_wind != 0: + wind_arrow_scale_factor = 1 # Scale factor for the wind arrow + wind = wind_arrow_scale_factor*wind / max_wind # Apply scaling on wind. + # Decimate data to render interval; always include t=0. if time[-1] != 0: sample_time = np.arange(0, time[-1], 1/render_fps * rtf) @@ -74,6 +83,7 @@ def animate(time, position, rotation, world, filename=None, blit=False, show_axe time = time[index] position = position[index,:] rotation = rotation[index,:,:] + wind = wind[index,:] # Set up axes. if filename is not None: @@ -91,7 +101,7 @@ def animate(time, position, rotation, world, filename=None, blit=False, show_axe ax.set_ylim(-1,1) ax.set_zlim(-1,1) - quad = Quadrotor(ax) + quad = Quadrotor(ax, wind=animate_wind) world_artists = world.draw(ax) @@ -103,7 +113,7 @@ def animate(time, position, rotation, world, filename=None, blit=False, show_axe def update(frame): title_artist.set_text('t = {:.2f}'.format(time[frame])) - quad.transform(position=position[frame,:], rotation=rotation[frame,:,:]) + quad.transform(position=position[frame,:], rotation=rotation[frame,:,:], wind=wind[frame,:]) [a.do_3d_projection(fig.canvas.get_renderer()) for a in quad.artists] return world_artists + list(quad.artists) + [title_artist] diff --git a/rotorpy/utils/plotter.py b/rotorpy/utils/plotter.py index d78000a..77a7272 100644 --- a/rotorpy/utils/plotter.py +++ b/rotorpy/utils/plotter.py @@ -183,7 +183,7 @@ class Plotter(): return - def animate_results(self, fname=None): + def animate_results(self, animate_wind, fname=None): """ Animate the results @@ -191,7 +191,7 @@ class Plotter(): # Animation (Slow) # Instead of viewing the animation live, you may provide a .mp4 filename to save. - ani = animate(self.time, self.x, self.R, world=self.world, filename=fname) + ani = animate(self.time, self.x, self.R, self.wind, animate_wind, world=self.world, filename=fname) plt.show() return diff --git a/rotorpy/utils/shapes.py b/rotorpy/utils/shapes.py index 5fc61f7..bea1572 100644 --- a/rotorpy/utils/shapes.py +++ b/rotorpy/utils/shapes.py @@ -269,9 +269,10 @@ class Quadrotor(): def __init__(self, ax, arm_length=0.125, rotor_radius=0.08, n_rotors=4, - shade=True, color=None): + shade=True, color=None, wind=True): self.ax = ax + self.wind_bool = wind # Apply same color to all rotor objects. if color is None: @@ -291,14 +292,23 @@ class Quadrotor(): 0.1*rotor_radius, shade=shade, color=color) for _ in range(n_rotors)] - self.artists = tuple(itertools.chain.from_iterable(r.artists for r in self.rotors)) - self.transform(np.zeros((3,)), np.identity(3)) + artists = [r.artists for r in self.rotors] + if self.wind_bool: + self.wind_vector = [self.ax.quiver(0,0,0,0,0,0, color='k')] + artists.append(self.wind_vector) + self.artists = tuple(itertools.chain.from_iterable(artists)) + + self.transform(np.zeros((3,)), np.identity(3), np.zeros((3,))) - def transform(self, position, rotation): + def transform(self, position, rotation, wind=np.array([1,0,0])): position.shape = (3,1) + wind.shape = (3,1) for (r, pos) in zip(self.rotors, self.rotor_position.T): pos.shape = (3,1) r.transform(np.matmul(rotation,pos)+position, rotation) + if self.wind_bool: + self.wind_vector[0].remove() + self.wind_vector = [self.ax.quiver(position[0], position[1], position[2], wind[0], wind[1], wind[2], color='r', linewidth=1.5)] if __name__ == '__main__': from axes3ds import Axes3Ds diff --git a/rotorpy/wind/default_winds.py b/rotorpy/wind/default_winds.py index 060c251..562ad74 100644 --- a/rotorpy/wind/default_winds.py +++ b/rotorpy/wind/default_winds.py @@ -104,7 +104,6 @@ class LadderWind(object): max := array of maximum wind speeds across each axis duration := array of durations for each step Nstep := array for the integer number of discretized steps between min and max across each axis - start_step := """ # Check the inputs for consistency, quit and raise a flag if the inputs aren't physically realizable