swangnice/rotor_py_control
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Added wind vector animation

Browse Source
This commit is contained in:
spencerfolk
2023-04-05 15:37:52 -04:00
parent ea80d4367a
commit 9f0f9493a9
7 changed files with 32 additions and 11 deletions
Download Patch File Download Diff File Expand all files Collapse all files

1
examples/basic_usage.py
View File

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

BIN
media/gusty.gif
View File

Binary file not shown.
Side by Side Swipe Overlay

Before

Width:  |  Height:  |  Size: 304 KiB

After

Width:  |  Height:  |  Size: 390 KiB

3
rotorpy/environments.py
View File

@@ -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)

16
rotorpy/utils/animate.py
View File

@@ -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]

4
rotorpy/utils/plotter.py
View File

@@ -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

18
rotorpy/utils/shapes.py
View File

@@ -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))
def transform(self, position, rotation):
self.transform(np.zeros((3,)), np.identity(3), np.zeros((3,)))
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

1
rotorpy/wind/default_winds.py
View File

@@ -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