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

Init

Browse Source
This commit is contained in:
spencerfolk
2023-03-15 15:38:14 -04:00
commit 4d7fca10e4
62 changed files with 5891 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files

11
rotorpy/wind/README.md Normal file
View File

@@ -0,0 +1,11 @@
# Wind Module
Currently winds are implemented as independent objects that take the current time and the vehicle's location and computes a wind vector at that location. Winds can be spatially varying, temporally varying, or both!
In `default_winds.py` we have a collection of common gust models, including sinusoid wind signals, constant wind, and discrete step changes in wind.
`spatial_winds.py` includes an example of a static spatial wind field, in which a column of air resembling a steady state wind tunnel can be produced.
`dryden_winds.py` contain a collection of models that uses the [Dryden Wind Turbulence Model](https://en.wikipedia.org/wiki/Dryden_Wind_Turbulence_Model) to produce stochastic wind patterns. The model is implemented using code from the external repository: `wind-dynamics` [(link)](https://github.com/goromal/wind-dynamics).
If you would like to create your own wind module, you can do so by using the template found in `wind_template.py`.

0
rotorpy/wind/__init__.py Normal file
View File

182
rotorpy/wind/default_winds.py Normal file
View File

@@ -0,0 +1,182 @@
import numpy as np
import sys
"""
Below are some default wind objects that might be useful inputs to the system.
"""
class NoWind(object):
"""
This wind profile is the trivial case of no wind. It will output
zero wind speed on all axes for all time.
Alternatively, you can use ConstantWind with wx=wy=wz=0.
"""
def __init__(self):
"""
Inputs:
Nothing
"""
def update(self, t, position):
"""
Given the present time and position of the multirotor, return the
current wind speed on all three axes.
The wind should be expressed in the world coordinates.
"""
return np.array([0,0,0])
class ConstantWind(object):
"""
This wind profile is constant both spatially and temporally.
Wind speed is specified on each axis.
"""
def __init__(self, wx, wy, wz):
"""
"""
self.wind = np.array([wx, wy, wz])
def update(self, t, position):
"""
Given the present time and position of the multirotor, return the
current wind speed on all three axes.
The wind should be expressed in the world coordinates.
"""
return self.wind
class SinusoidWind(object):
"""
Wind will vary subject to a sine function with appropriate amplitude, frequency, and phase offset.
"""
def __init__(self, amplitudes=np.array([1,1,1]), frequencies=np.array([1,1,1]), phase=np.array([0,0,0])):
"""
Inputs:
amplitudes := array of amplitudes on each axis
frequencies := array of frequencies for the wind pattern on each axis
phase := relative phase offset on each axis in seconds
"""
self.Ax, self.Ay, self.Az = amplitudes[0], amplitudes[1], amplitudes[2]
self.fx, self.fy, self.fz = frequencies[0], frequencies[1], frequencies[2]
self.px, self.py, self.pz = phase[0], phase[1], phase[2]
def update(self, t, position):
"""
Given the present time and position of the multirotor, return the
current wind speed on all three axes.
The wind should be expressed in the world coordinates.
"""
wind = np.array([self.Ax*np.sin(2*np.pi*self.fx*(t+self.px)),
self.Ay*np.sin(2*np.pi*self.fy*(t+self.py)),
self.Az*np.sin(2*np.pi*self.fz*(t+self.pz))])
return wind
class LadderWind(object):
"""
The wind will step up and down between a minimum and maximum speed for a specified duration.
Visualized below...
| | <- duration
max -> --- ---
--- ---
--- ---
min -> --- ---
--------------------------> t
** Normally the wind will start at min and increase sequentially, but if the random flag is set true,
the wind will step to a random sublevel after each duration is up.
"""
def __init__(self, min=np.array([-1,-1,-1]), max=np.array([1,1,1]), duration=np.array([1,1,1]), Nstep=np.array([5,5,5]), random_flag=False):
"""
Inputs:
min := array of minimum wind speeds across each axis
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
if np.any(Nstep <= 0):
print("LadderWind Error: The number of steps must be greater than or equal to 1")
sys.exit(1)
if np.any(max - min < 0):
print("LadderWind Error: The max value must be greater than the min value.")
sys.exit(1)
self.random_flag = random_flag
self.durx, self.dury, self.durz = duration[0], duration[1], duration[2]
self.nx, self.ny, self.nz = Nstep[0], Nstep[1], Nstep[2]
# Compute arrays of intermediate wind speeds for each axis
self.wx_arr = np.linspace(min[0], max[0], self.nx)
self.wy_arr = np.linspace(min[1], max[1], self.ny)
self.wz_arr = np.linspace(min[2], max[2], self.nz)
# Initialize the amplitude id.. these numbers are used to index the arrays above to get the appropriate wind speed on each axis
if self.random_flag:
self.xid = np.random.choice(self.nx)
self.yid = np.random.choice(self.ny)
self.zid = np.random.choice(self.nz)
else:
self.xid, self.yid, self.zid = 0, 0, 0
# Initialize the timers... since we don't yet know the starting time, we'll set them in the first call
self.timerx = None
self.timery = None
self.timerz = None
# Initialize the winds
self.wx, self.wy, self.wz = self.wx_arr[self.xid], self.wy_arr[self.yid], self.wz_arr[self.zid]
def update(self, t, position):
"""
Given the present time and position of the multirotor, return the
current wind speed on all three axes.
The wind should be expressed in the world coordinates.
"""
if self.timerx is None:
self.timerx, self.timery, self.timerz = t, t, t
if (t - self.timerx) >= self.durx:
if self.random_flag:
self.xid = np.random.choice(self.nx)
else:
self.xid = (self.xid + 1) % self.nx
self.wx = self.wx_arr[self.xid]
self.timerx = t
if (t - self.timery) >= self.dury:
if self.random_flag:
self.yid = np.random.choice(self.ny)
else:
self.yid = (self.yid + 1) % self.ny
self.wy = self.wy_arr[self.yid]
self.timery = t
if (t - self.timerz) >= self.durz:
if self.random_flag:
self.zid = np.random.choice(self.nz)
else:
self.zid = (self.zid + 1) % self.nz
self.wz = self.wz_arr[self.zid]
self.timerz = t
return np.array([self.wx, self.wy, self.wz])
if __name__=="__main__":
wind = ConstantWind(wx=1,wy=1,wz=1)
print(wind.update(0,np.array([0,0,0])))

151
rotorpy/wind/dryden_utils.py Normal file
View File

@@ -0,0 +1,151 @@
"""
Utility scripts for the Dryden wind model
The Dryden Gust model is implemented using this package:
https://github.com/goromal/wind-dynamics
"""
import numpy as np
class GustModelBase():
"""
"""
def __init__(self, V, L, sigma, dt=0.05):
"""
Inputs:
V, average velocity through medium, m/s
L, characteristic length scale, m
sigma, turbulence intensity
"""
self.dt = dt
b = 2*np.sqrt(3)*L/V
c = 2*L/V
self.alpha = sigma*np.sqrt(2*L/np.pi/V)
self.beta = self.alpha*b
self.delta = 2*c
self.gamma = c*c
self.u_km1 = 0
self.u_km2 = 0
self.y_km1 = 0
self.y_km2 = 0
self.initialized = True
return
def run(self, dt):
"""
"""
C1 = 1.0 + 2*self.delta/dt + 4*self.gamma/dt/dt
C2 = 2.0 - 8*self.gamma/dt/dt
C3 = 1.0 - 2*self.delta/dt + 4*self.gamma/dt/dt
C4 = self.alpha + 2*self.beta/dt
C5 = 2*self.alpha
C6 = self.alpha - 2*self.beta/dt
u_k = np.random.uniform(-1,1)
y_k = (C4*u_k + C5*self.u_km1 + C6*self.u_km2 - C2*self.y_km1 - C3*self.y_km2)/C1
self.u_km2 = self.u_km1
self.u_km1 = u_k
self.y_km2 = self.y_km1
self.y_km1 = y_k
return y_k
def integrate(self, dt):
"""
"""
if (dt > self.dt):
t = 0
y = 0
while (t < dt):
t_inc = min(self.dt, dt - t)
y = self.run(t_inc)
t += t_inc
return y
else:
return self.run(dt)
class DrydenWind():
"""
"""
def __init__(self, wx_nominal, wy_nominal, wz_nominal,
wx_sigma, wy_sigma, wz_sigma,
altitude=2.0):
self.wx_nominal, self.wy_nominal, self.wz_nominal = wx_nominal, wy_nominal, wz_nominal
self.altitude = altitude
Lz_ft = 3.281 * altitude
Lx_ft = Lz_ft / ((0.177 + 0.000823 * Lz_ft)**(1.2))
Ly_ft = Lx_ft
self.wx_gust = GustModelBase(1.0, Lx_ft/3.281, wx_sigma)
self.wy_gust = GustModelBase(1.0, Ly_ft/3.281, wy_sigma)
self.wz_gust = GustModelBase(1.0, Lz_ft/3.281, wz_sigma)
self.initialized = True
return
def getWind(self, dt):
"""
"""
if self.initialized:
wind_vector = np.array([self.wx_nominal, self.wy_nominal, self.wz_nominal]) + np.array([self.wx_gust.integrate(dt), self.wy_gust.integrate(dt), self.wz_gust.integrate(dt)])
else:
wind_vector = np.array([0,0,0])
return wind_vector
if __name__=="__main__":
import matplotlib.pyplot as plt
import numpy as np
t = 100.0
dt = 0.05
x_mean = 0.0
y_mean = 1.0
z_mean = 0.0
x_sigma = 10.0
y_sigma = 0.0
z_sigma = 70.0
plt.rcParams.update({
"text.usetex": True,
"font.family": "serif",
"font.serif": ["Palatino"],
})
wind = DrydenWind(x_mean, y_mean, z_mean, x_sigma, y_sigma, z_sigma)
n = int(np.floor(t / dt)) + 1
x = np.linspace(0, t, n)
y = np.zeros((3,x.size))
for i in range(n):
y[:,i] = wind.getWind(dt)
fig, axs = plt.subplots(3)
fig.suptitle('Dryden Gust Velocities\n$\sigma_x=%f$, $\sigma_y=%f$, $\sigma_z=%f$' % (x_sigma, y_sigma, z_sigma))
axs[0].plot([0, t],[x_mean, x_mean])
axs[0].plot(x, y[0,:])
axs[0].set_ylabel('X Velocity')
axs[1].plot([0, t],[y_mean, y_mean])
axs[1].plot(x, y[1,:])
axs[1].set_ylabel('Y Velocity')
axs[2].plot([0, t],[z_mean, z_mean])
axs[2].plot(x, y[2,:])
axs[2].set_ylabel('Z Velocity')
axs[2].set_xlabel('Time (s)')
plt.show()

85
rotorpy/wind/dryden_winds.py Normal file
View File

@@ -0,0 +1,85 @@
import numpy as np
import os
import sys
# The Dryden Gust model is implemented using this package:
# https://github.com/goromal/wind-dynamics
# If using the package directly, make sure the package is installed and the file ./wind-dynamics/python/wind_dynamics.so
# exists. It may be named slightly differently depending on your system.
# wind_path = os.path.join(os.path.dirname(__file__),'wind-dynamics/python')
# sys.path.insert(1, wind_path) # if your script is not in the wind-dynamics/python directory
# from wind_dynamics import DrydenWind
from rotorpy.wind.dryden_utils import *
class DrydenGust(object):
"""
The Dryden Wind Turbulence model is governed by a pink noise process that is parameterized by
an average (mean) windspeed and standard deviation. This class is a wrapper on an
existing external package: https://github.com/goromal/wind-dynamics
The Dryden model is accepted by the DoD in design, characterization, and testing of aircraft
in simulation. https://en.wikipedia.org/wiki/Dryden_Wind_Turbulence_Model
"""
def __init__(self, dt=1/500,
avg_wind=np.array([0,0,0]), sig_wind=np.array([1,1,1]),
altitude=2.0):
"""
Inputs:
dt := time discretization, s, should match the simulator
avg_wind := mean windspeed on each axis, m/s
sig_wind := turbulence intensity in windspeed on each axis, m/s
altitude := expected operating altitude
"""
self.dt = dt
self.wind = DrydenWind(avg_wind[0], avg_wind[1], avg_wind[2], sig_wind[0], sig_wind[1], sig_wind[2], altitude)
def update(self, t, position):
"""
Given the present time and position of the multirotor, return the
current wind speed on all three axes.
The wind should be expressed in the world coordinates.
"""
return self.wind.getWind(self.dt)
class DrydenGustLP(object):
"""
This is another wrapper on an existing external package: https://github.com/goromal/wind-dynamics
The Dryden model is accepted by the DoD in design, characterization, and testing of aircraft
in simulation. https://en.wikipedia.org/wiki/Dryden_Wind_Turbulence_Model
The difference between this model and DrydenGust is that we add an additional low pass filter governed by
a cutoff frequency, 1/tau in order to generate smoother varying winds.
"""
def __init__(self, dt=1/500,
avg_wind=np.array([0,0,0]), sig_wind=np.array([1,1,1]),
altitude=2.0,
tau=0.1):
"""
Inputs:
dt := time discretization, s, should match the simulator
avg_wind := mean windspeed on each axis, m/s
sig_wind := turbulence intensity (denoted sigma) in windspeed on each axis, m/s
altitude := expected operating altitude
tau := cutoff frequency of the low pass filter (s)
"""
self.dt = dt
self.tau = tau
self.wind = DrydenWind(avg_wind[0], avg_wind[1], avg_wind[2], sig_wind[0], sig_wind[1], sig_wind[2], altitude)
self.prev_wind = self.wind.getWind(self.dt)
def update(self, t, position):
"""
Given the present time and position of the multirotor, return the
current wind speed on all three axes.
The wind should be expressed in the world coordinates.
"""
wind = (1-self.dt/self.tau)*self.prev_wind + self.dt/self.tau*self.wind.getWind(self.dt)
self.prev_wind = wind
return wind

44
rotorpy/wind/spatial_winds.py Normal file
View File

@@ -0,0 +1,44 @@
"""
Here is an example of a static spatial wind field.
"""
import numpy as np
class WindTunnel(object):
"""
This profile is a cylindrical column of air pointing at (0,0,0). Outside of this column the wind is zero.
"""
def __init__(self, mag=1, dir=np.array([1,0,0]), radius=1):
"""
Inputs:
magnitude, a scalar representing the strength of the wind column
direction, a vector describing the direction of the wind column
e.g., [1,0,0] means the column will be directed in the positive x axis
radius, the size of the column (the radius of the cylinder) in meters.
"""
self.mag = mag
if np.linalg.norm(dir) > 1: # Check if this is a unit vector. If not, normalize it.
self.dir = dir/np.linalg.norm(dir)
else:
self.dir = dir
self.radius = radius
def update(self, t, position):
"""
Given the present time and position of the vehicle in the world frame, return the
current wind speed on all three axes.
"""
# We can get the perpendicular distance of the vector xi+yj+zk from the line self.dir
# using the cross product. This assumes that ||self.dir|| = 1 and that the column
# passes through the origin (0,0,0).
if np.linalg.norm(np.cross(position, self.dir)) <= self.radius:
# If this condition passes, we're within the column.
wind = self.mag*self.dir
else:
wind = np.array([0,0,0])
return wind

35
rotorpy/wind/wind_template.py Normal file
View File

@@ -0,0 +1,35 @@
"""
Wind profiles should be implemented here to be added as an argument in the
simulate method found in simulate.py
The main structure of each object is the only thing that must remain consistent.
When creating a new wind profile, the object must have the following:
- An __init__ method. The arguments vary depending on the wind profile.
- An update method. The arguments *must* include time and position.
"""
import numpy as np
class WindTemplate(object):
"""
Winds are implemented with two required methods: __init__() and update().
With __init__() you can specify any parameters or constants used to specify the wind.
The update() method is called at each time step of the simulator. The position and time
of the vehicle are provided.
"""
def __init__(self):
"""
Inputs:
Nothing
"""
def update(self, t, position):
"""
Given the present time and position of the vehicle in the world frame, return the
current wind speed on all three axes.
Be careful whether or not the wind is specified in the body or world coordinates!.
"""
return np.array([0,0,0])