Added wind vector animation

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/. 
                     )

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)

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]
 

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

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))
+        artists = [r.artists for r in self.rotors]
-        self.transform(np.zeros((3,)), np.identity(3))
+        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

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