def smallAngleApprox(self, angle_list, extraSubs=[]):
"""
Apply small angle approximation to forcing and mass matrix
"""
# Forcing
with Timer('Small angle approx. forcing', True):
if self._sa_forcing is None:
self._sa_forcing = self.kane.forcing
self._sa_forcing = self._sa_forcing.subs(
self.kdeqsSubs).subs(extraSubs)
self._sa_forcing = smallAngleApprox(self._sa_forcing, angle_list)
with Timer('Small angle approx. forcing simplify', True):
self._sa_forcing.simplify()
# Mass matrix
with Timer('Small angle approx. mass matrix', True):
if self._sa_mass_matrix is None:
self._sa_mass_matrix = self.kane.mass_matrix
self._sa_mass_matrix = self._sa_mass_matrix.subs(
self.kdeqsSubs).subs(extraSubs)
self._sa_mass_matrix = smallAngleApprox(self._sa_mass_matrix,
angle_list)
with Timer('Small angle approx. mass matrix simplify', True):
self._sa_mass_matrix.simplify()
self.smallAngleUsed = angle_list
def kaneEquations(self, Mform='symbolic', addGravity=True):
"""
Compute equation of motions using Kane's method
Mform: form to use for mass matrix
addGravity: include gravity for elastic bodies
"""
for sa in ['ref', 'coordinates', 'speeds', 'kdeqs', 'bodies', 'loads']:
if getattr(self, sa) is None:
raise Exception(
'Attribute {} needs to be set before calling `kane` method'
.format(sa))
with Timer('Kane step1', True):
self.kane = YAMSKanesMethod(self.ref.frame, self.coordinates,
self.speeds, self.kdeqs)
# --- Expensive kane step
with Timer('Kane step 2', True):
#(use Mform ='symbolic' or 'TaylorExpanded'), Mform='symbolic'
self.fr, self.frstar = self.kane.kanes_equations(
self.bodies,
self.loads,
Mform=Mform,
addGravity=addGravity,
g_vect=self.g_vect)
self.kane.fr = self.fr
self.kane.frstar = self.frstar
def linearizeQ(EOM, q, op_point=[], noAcc=True, noVel=False, extraSubs=[]):
qd = [qi.diff(dynamicsymbols._t) for qi in q]
qdd = [qdi.diff(dynamicsymbols._t) for qdi in qd]
with Timer('Linearization', True):
# NOTE: order important
op_point = []
if noAcc:
op_point += [(qddi, 0) for qddi in qdd]
if noVel:
op_point += [(qdi, 0) for qdi in qd]
# --- Inputs are dynamic symbols that are not coordinates
dyn_symbols = find_dynamicsymbols(EOM)
all_q = q + qd + qdd
inputs = [s for s in dyn_symbols if s not in all_q]
II = np.argsort([str(v) for v in inputs]) # sorting for consistency
inputs = list(np.array(inputs)[II])
# KEEP ME Alternative
#M, A, B = linearizer.linearize(op_point=op_point ) #o
M = -EOM.jacobian(qdd).subs(extraSubs).subs(op_point)
C = -EOM.jacobian(qd).subs(extraSubs).subs(op_point)
K = -EOM.jacobian(q).subs(extraSubs).subs(op_point)
if len(inputs) > 0:
B = EOM.jacobian(inputs).subs(extraSubs).subs(op_point)
else:
B = Matrix([])
return M, C, K, B, inputs
def smallAngleApprox(self, angle_list, order=1):
"""
Apply small angle approximation to EOM
"""
with Timer('Small angle approx', True):
self.EOM = smallAngleApprox(self.EOM, angle_list, order=order)
self.smallAngleUsed += angle_list
def smallAngleApproxEOM(self, angle_list, extraSubs=[]):
"""
Apply small angle approximation to equation of motion H(x,xd,xdd,..)=0
"""
EOM = self.EOM
with Timer('Small angle approx. EOM', True):
EOM = EOM.subs(extraSubs)
EOM = smallAngleApprox(EOM, angle_list).subs(extraSubs)
#with Timer('Small angle approx. EOM simplify',True):
# EOM.simplify()
self._sa_EOM = EOM
def _linearize(self,
op_point=[],
EOM=None,
noAcc=True,
noVel=False,
extraSubs=[]):
if EOM is None:
EOM = self.EOM
with Timer('Linearization', True):
# NOTE: order important
op_point = []
if noAcc:
op_point += [(qdd, 0) for qdd in self.coordinates_acc]
op_point += [(diff(qd, dynamicsymbols._t), 0)
for qd in self.speeds]
if noVel:
op_point += [(qd, 0) for qd in self.coordinates_speed]
op_point += [(qd, 0) for qd in self.speeds]
# --- Using kane to get "r"...
linearizer = self.kane.to_linearizer()
self.var = self.kane._lin_r
II = np.argsort([str(v) for v in self.var])
self.var = list(np.array(self.var)[II])
# KEEP ME Alternative
#M, A, B = linearizer.linearize(op_point=op_point ) #o
M = -EOM.jacobian(
self.coordinates_acc).subs(extraSubs).subs(op_point)
C = -EOM.jacobian(
self.coordinates_speed).subs(extraSubs).subs(op_point)
K = -EOM.jacobian(self.coordinates).subs(extraSubs).subs(op_point)
if len(self.var) > 0:
B = EOM.jacobian(self.var).subs(extraSubs).subs(op_point)
else:
B = Matrix([])
return M, C, K, B
ax.tick_params(direction='in')
ax.autoscale(enable=True, axis='both', tight=True)
ax.set_xlabel('Time [s]')
ax.set_ylabel(r'Wind speed [m/s]')
ax.set_title('Wind - wind generation at point')
ax = axes[1]
ax.plot(f_fft, S_fft, '-', label='Generated')
ax.plot(freq, S, 'k', label='Kaimal')
ax.set_yscale('log')
ax.set_xscale('log')
ax.legend()
ax.set_xlabel('Frequency [Hz]')
ax.set_ylabel(r'Spectral density [m$^2$ s]')
ax.tick_params(direction='in')
ax.autoscale(enable=True, axis='both', tight=True)
if __name__ == '__main__':
from welib.tools.tictoc import Timer
with Timer('ifft'):
generateTSPlot(method='ifft')
#generateTS(method='sum')
plt.show()
if __name__ == '__export__':
generateTSPlot(method='ifft')
from welib.tools.repo import export_figs_callback
export_figs_callback(__file__)
if __name__ == '__main__':
from welib.tools.spectral import fft_wrap
from welib.tools.tictoc import Timer
seed(11)
U0 = 8 # Wind speed [m/s], for Kaimal spectrum
I = 0.14 # Turbulence intensity [-], for Kaimal spectrum
L = 340.2 # Length scale [m], for Kaimal spectrum
tMax = 800 # Maximum time for time series [s]
dt = 0.01 # Time step [s]
for method in ['sum','ifft']:
with Timer(method):
t, u, freq, S =pointTSKaimal(tMax, dt, U0, U0*I, L, method=method)
# --- Compute FFT of wind speed
f_fft, S_fft, Info = fft_wrap(t, u, output_type='PSD', averaging='none')
# --- Plots
import matplotlib.pyplot as plt
fig,axes = plt.subplots(2, 1, sharey=False, figsize=(6.4,4.8)) # (6.4,4.8)
fig.subplots_adjust(left=0.12, right=0.95, top=0.95, bottom=0.11, hspace=0.30, wspace=0.20)
ax=axes[0]
ax.plot(t, u)
ax.tick_params(direction='in')
ax.autoscale(enable=True, axis='both', tight=True)
ax.set_xlabel('Time [s]')
def savePython(self,
prefix='',
suffix='',
folder='./',
extraSubs=[],
replaceDict=None,
doSimplify=False,
velSubs=[(0, 0)]):
""" Export EOM to a python file (will be rewritten) """
name = prefix + self.name
if len(suffix) > 0:
name = name + '_' + suffix.strip('_')
filename = os.path.join(folder, name + '.py')
# --- replace dict
if replaceDict is None:
replaceDict = OrderedDict()
replaceDict.update(self.bodyReplaceDict)
#print(replaceDict)
with Timer('Python to {}'.format(filename), True):
with open(filename, 'w') as f:
f.write('"""\n')
f.write('Equations of motion\n')
f.write('model name: {}\n'.format(self.name))
f.write('"""\n')
f.write('import numpy as np\n')
f.write('from numpy import cos, sin\n')
infoToPy(f, self.name, self.q, self.input_vars)
if self.M is not None:
forcingToPy(f,
self.q,
self.F,
replaceDict=replaceDict,
extraSubs=extraSubs,
velSubs=velSubs,
doSimplify=doSimplify)
MMToPy(f,
self.q,
self.M,
replaceDict=replaceDict,
extraSubs=extraSubs,
velSubs=velSubs,
doSimplify=doSimplify)
if self.M0 is not None:
M0ToPy(f,
self.q,
self.M0,
replaceDict=replaceDict,
extraSubs=extraSubs,
velSubs=velSubs,
doSimplify=doSimplify)
C0ToPy(f,
self.q,
self.C0,
replaceDict=replaceDict,
extraSubs=extraSubs,
velSubs=velSubs,
doSimplify=doSimplify)
K0ToPy(f,
self.q,
self.K0,
replaceDict=replaceDict,
extraSubs=extraSubs,
velSubs=velSubs,
doSimplify=doSimplify)
B0ToPy(f,
self.q,
self.B0,
self.input_vars,
replaceDict=replaceDict,
extraSubs=extraSubs,
velSubs=velSubs,
doSimplify=doSimplify)
def saveTex(self,
prefix='',
suffix='',
folder='./',
extraSubs=[],
header=True,
extraHeader=None,
variables=['M', 'F', 'M0', 'C0', 'K0', 'B0'],
doSimplify=False,
velSubs=[(0, 0)]):
"""
Save EOM to a latex file
"""
name = prefix + self.name
if len(suffix) > 0:
name = name + '_' + suffix.strip('_')
filename = os.path.join(folder, name + '.tex')
with Timer('Latex to {}'.format(filename), True):
with open(filename, 'w') as f:
if header:
f.write('Model: {}, \n'.format(self.name.replace(
'_', '\_')))
f.write('Degrees of freedom: ${}$, \n'.format(
cleantex(self.q)))
try:
f.write('Small angles: ${}$\\\\ \n'.format(
cleantex(self.smallAngleUsed)))
except:
pass
f.write('Free vars: ${}$\\\\ \n'.format(
cleantex(self.input_vars)))
if extraHeader:
f.write('\\clearpage\n{}\\\\\n'.format(extraHeader))
if 'F' in variables:
toTex(f,
self.F,
label='Forcing',
fullPage=True,
extraSubs=extraSubs,
velSubs=velSubs,
doSimplify=doSimplify)
if 'M' in variables:
toTex(f,
self.M,
label='Mass matrix',
fullPage=True,
extraSubs=extraSubs,
velSubs=velSubs,
doSimplify=doSimplify)
if 'M0' in variables:
toTex(f,
self.M0,
label='Linearized mass matrix',
fullPage=True,
extraSubs=extraSubs,
velSubs=velSubs,
doSimplify=doSimplify)
if 'C0' in variables:
toTex(f,
self.C0,
label='Linearized damping matrix',
fullPage=True,
extraSubs=extraSubs,
velSubs=velSubs,
doSimplify=doSimplify)
if 'K0' in variables:
toTex(f,
self.K0,
label='Linearized stiffness matrix',
fullPage=True,
extraSubs=extraSubs,
velSubs=velSubs,
doSimplify=doSimplify)
if 'B0' in variables:
toTex(f,
self.B0,
label='Linearized forcing matrix',
fullPage=True,
extraSubs=extraSubs,
velSubs=velSubs,
doSimplify=doSimplify)
def simplify(self):
""" Simplify equations of motion """
with Timer('Simplify', True):
self.EOM.simplify()
def savePython(self,
prefix='',
suffix='',
folder='./',
extraSubs=[],
variables=[
'MM', 'FF', 'MMsa', 'FFsa', 'M', 'C', 'K', 'B', 'Msa',
'Csa', 'Ksa', 'Bsa'
],
replaceDict=None,
doSimplify=False,
velSubs=[(0, 0)]):
"""
Save forcing, mass matrix and linear model to python package
"""
name = prefix + self.name
if len(suffix) > 0:
name = name + '_' + suffix.strip('_')
filename = os.path.join(folder, name + '.py')
# --- replace dict
if replaceDict is None:
replaceDict = OrderedDict()
for b in self.bodies:
if isinstance(b, YAMSFlexibleBody):
b.replaceDict(replaceDict)
#print(replaceDict)
with Timer('Python to {}'.format(filename), True):
with open(filename, 'w') as f:
f.write('"""\n')
f.write('{}\n'.format(self.__repr__()))
f.write('"""\n')
f.write('import numpy as np\n')
f.write('from numpy import cos, sin\n')
infoToPy(f, self.name, self.coordinates, self.var)
if 'FF' in variables:
forcing = self.kane.forcing.subs(self.kdeqsSubs)
forcingToPy(f,
self.coordinates,
forcing,
replaceDict=replaceDict,
extraSubs=extraSubs,
velSubs=velSubs,
doSimplify=doSimplify)
if 'MM' in variables:
MM = self.kane.mass_matrix.subs(self.kdeqsSubs)
MMToPy(f,
self.coordinates,
MM,
replaceDict=replaceDict,
extraSubs=extraSubs,
velSubs=velSubs,
doSimplify=doSimplify)
if 'M' in variables:
M0ToPy(f,
self.coordinates,
self.M,
replaceDict=replaceDict,
extraSubs=extraSubs,
velSubs=velSubs,
doSimplify=doSimplify)
if 'C' in variables:
C0ToPy(f,
self.coordinates,
self.M,
replaceDict=replaceDict,
extraSubs=extraSubs,
velSubs=velSubs,
doSimplify=doSimplify)
if 'K' in variables:
K0ToPy(f,
self.coordinates,
self.M,
replaceDict=replaceDict,
extraSubs=extraSubs,
velSubs=velSubs,
doSimplify=doSimplify)
if 'B' in variables:
B0ToPy(f,
self.coordinates,
self.M,
self.var,
replaceDict=replaceDict,
extraSubs=extraSubs,
velSubs=velSubs,
doSimplify=doSimplify)
if 'FFsa' in variables:
FF = subs_no_diff(self._sa_forcing, extraSubs)
FF = FF.subs(velSubs)
if doSimplify:
FF = FF.simplify()
s, params, inputs, sdofs = cleanPy(FF,
varname='FF',
dofs=self.coordinates,
indent=4,
replDict=replaceDict)
f.write(
'def forcing_sa(t,q=None,qd=None,p=None,u=None,z=None):\n'
)
f.write(
' """ Non linear forcing with small angle approximation\n'
)
f.write(
' q: degrees of freedom, array-like: {}\n'.format(
sdofs))
f.write(' qd: dof velocities, array-like\n')
f.write(' p: parameters, dictionary with keys: {}\n'.
format(params))
f.write(
' u: inputs, dictionary with keys: {}\n'.format(
inputs))
f.write(
' where each values is a function of time\n')
f.write(' """\n')
f.write(' if z is not None:\n')
f.write(' q = z[0:int(len(z)/2)] \n')
f.write(' qd = z[int(len(z)/2): ] \n')
f.write(s)
f.write(' return FF\n\n')
if 'MMsa' in variables:
MM = subs_no_diff(self._sa_mass_matrix, extraSubs)
MM = MM.subs(velSubs)
if doSimplify:
MM.simplify()
s, params, inputs, sdofs = cleanPy(MM,
varname='MM',
dofs=self.coordinates,
indent=4,
replDict=replaceDict)
f.write('def mass_matrix_sa(q=None,p=None,z=None):\n')
f.write(
' """ Non linear mass matrix with small angle approximation\n'
)
f.write(
' q: degrees of freedom, array-like: {}\n'.format(
sdofs))
f.write(' p: parameters, dictionary with keys: {}\n'.
format(params))
f.write(' """\n')
f.write(' if z is not None:\n')
f.write(' q = z[0:int(len(z)/2)] \n')
f.write(s)
f.write(' return MM\n\n')
if 'Msa' in variables:
MM = subs_no_diff(self._sa_M, extraSubs)
MM = MM.subs(velSubs)
if doSimplify:
MM.simplify()
s, params, inputs, sdofs = cleanPy(MM,
varname='MM',
dofs=self.coordinates,
indent=4,
replDict=replaceDict,
noTimeDep=True)
f.write('def M_lin_sa(q=None,p=None,z=None):\n')
f.write(
' """ Linear mass matrix with small angle approximation\n'
)
f.write(
' q: degrees of freedom at operating point, array-like: {}\n'
.format(sdofs))
f.write(' p: parameters, dictionary with keys: {}\n'.
format(params))
f.write(' """\n')
f.write(' if z is not None:\n')
f.write(' q = z[0:int(len(z)/2)] \n')
f.write(s)
f.write(' return MM\n\n')
if 'Csa' in variables:
MM = subs_no_diff(self._sa_C, extraSubs)
MM = MM.subs(velSubs)
if doSimplify:
MM.simplify()
s, params, inputs, sdofs = cleanPy(MM,
varname='CC',
dofs=self.coordinates,
indent=4,
replDict=replaceDict,
noTimeDep=True)
f.write(
'def C_lin_sa(q=None,qd=None,p=None,u=None,z=None):\n')
f.write(
' """ Linear damping matrix with small angle approximation\n'
)
f.write(
' q: degrees of freedom at operating point, array-like: {}\n'
.format(sdofs))
f.write(
' qd: dof velocities at operating point, array-like\n'
)
f.write(' p: parameters, dictionary with keys: {}\n'.
format(params))
f.write(
' u: inputs at operating point, dictionary with keys: {}\n'
.format(inputs))
f.write(' where each values is a constant!\n')
f.write(' """\n')
f.write(' if z is not None:\n')
f.write(' q = z[0:int(len(z)/2)] \n')
f.write(' qd = z[int(len(z)/2): ] \n')
f.write(s)
f.write(' return CC\n\n')
if 'Ksa' in variables:
MM = subs_no_diff(self._sa_K, extraSubs)
MM = MM.subs(velSubs)
if doSimplify:
MM.simplify()
s, params, inputs, sdofs = cleanPy(MM,
varname='KK',
dofs=self.coordinates,
indent=4,
replDict=replaceDict,
noTimeDep=True)
f.write(
'def K_lin_sa(q=None,qd=None,p=None,u=None,z=None):\n')
f.write(
' """ Linear stiffness matrix with small angle approximation\n'
)
f.write(
' q: degrees of freedom, array-like: {}\n'.format(
sdofs))
f.write(' qd: dof velocities, array-like\n')
f.write(' p: parameters, dictionary with keys: {}\n'.
format(params))
f.write(
' u: inputs at operating point, dictionary with keys: {}\n'
.format(inputs))
f.write(' where each values is a constant!\n')
f.write(' """\n')
f.write(' if z is not None:\n')
f.write(' q = z[0:int(len(z)/2)] \n')
f.write(' qd = z[int(len(z)/2): ] \n')
f.write(s)
f.write(' return KK\n\n')
if 'Bsa' in variables:
MM = subs_no_diff(self._sa_B, extraSubs)
MM = MM.subs(velSubs)
if doSimplify:
MM.simplify()
s, params, inputs, sdofs = cleanPy(MM,
varname='BB',
dofs=self.coordinates,
indent=4,
replDict=replaceDict,
noTimeDep=True)
f.write('def B_lin_sa(q=None,qd=None,p=None,u=None):\n')
f.write(
' """ Linear mass matrix with small angle approximation\n'
)
f.write(
' q: degrees of freedom at operating point, array-like: {}\n'
.format(sdofs))
f.write(
' qd: dof velocities at operating point, array-like\n'
)
f.write(' p: parameters, dictionary with keys: {}\n'.
format(params))
f.write(
' u: inputs at operating point, dictionary with keys: {}\n'
.format(inputs))
f.write(' where each values is a constant!\n')
f.write(' The columns of B correspond to: {}\\\\ \n'.
format(self.var))
f.write(' """\n')
f.write(s)
f.write(' return BB\n\n')
def saveTex(self,
prefix='',
suffix='',
folder='./',
extraSubs=[],
header=True,
extraHeader=None,
variables=[
'MM', 'FF', 'M', 'C', 'K', 'B', 'MMsa', 'FFsa', 'Msa',
'Csa', 'Ksa', 'Bsa'
],
doSimplify=False,
velSubs=[(0, 0)]):
"""
Save forcing and mass matrix to latex file
"""
name = prefix + self.name
if len(suffix) > 0:
name = name + '_' + suffix.strip('_')
filename = os.path.join(folder, name + '.tex')
with Timer('Latex to {}'.format(filename), True):
with open(filename, 'w') as f:
if header:
f.write('Model: {}, \n'.format(self.name.replace(
'_', '\_')))
f.write('Degrees of freedom: ${}$, \n'.format(
cleantex(self.coordinates)))
try:
f.write('Small angles: ${}$\\\\ \n'.format(
cleantex(self.smallAngleUsed)))
except:
pass
f.write('Free vars: ${}$\\\\ \n'.format(
cleantex(self.var)))
if extraHeader:
f.write('\\clearpage\n{}\\\\\n'.format(extraHeader))
if 'F' in variables:
FF = self.kane.forcing.subs(self.kdeqsSubs)
toTex(f,
FF,
label='Forcing',
fullPage=True,
extraSubs=extraSubs,
velSubs=velSubs,
doSimplify=doSimplify)
if 'M' in variables:
MM = self.kane.mass_matrix.subs(
self.kdeqsSubs) # NOTE this is wrong
toTex(f,
MM,
label='Mass matrix',
fullPage=True,
extraSubs=extraSubs,
velSubs=velSubs,
doSimplify=doSimplify)
if 'M0' in variables:
toTex(f,
self.M0,
label='Linearized mass matrix',
fullPage=True,
extraSubs=extraSubs,
velSubs=velSubs,
doSimplify=doSimplify)
if 'C0' in variables:
toTex(f,
self.C0,
label='Linearized damping matrix',
fullPage=True,
extraSubs=extraSubs,
velSubs=velSubs,
doSimplify=doSimplify)
if 'K0' in variables:
toTex(f,
self.K0,
label='Linearized stiffness matrix',
fullPage=True,
extraSubs=extraSubs,
velSubs=velSubs,
doSimplify=doSimplify)
if 'B0' in variables:
toTex(f,
self.B0,
label='Linearized forcing matrix',
fullPage=True,
extraSubs=extraSubs,
velSubs=velSubs,
doSimplify=doSimplify)
if 'FFsa' in variables:
FF = subs_no_diff(self._sa_forcing, extraSubs)
toTex(f,
FF,
label='Forcing small angle',
fullPage=True,
extraSubs=extraSubs,
velSubs=velSubs,
doSimplify=doSimplify)
if 'MMsa' in variables:
toTex(f,
self._sa_mass_matrix,
label='Mass matrix small angle',
fullPage=True,
extraSubs=extraSubs,
velSubs=velSubs,
doSimplify=doSimplify)
if 'Msa' in variables:
toTex(f,
self._sa_M,
label='Linearized mass matrix small angle',
fullPage=True,
extraSubs=extraSubs,
velSubs=velSubs,
doSimplify=doSimplify)
if 'Csa' in variables:
toTex(f,
self._sa_C,
label='Linearized damping matrix small angle',
fullPage=True,
extraSubs=extraSubs,
velSubs=velSubs,
doSimplify=doSimplify)
if 'Ksa' in variables:
toTex(f,
self._sa_K,
label='Linearized stiffness matrix small angle',
fullPage=True,
extraSubs=extraSubs,
velSubs=velSubs,
doSimplify=doSimplify)
if 'Bsa' in variables:
toTex(f,
self._sa_K,
label='Linearized forcing small angle',
fullPage=True,
extraSubs=extraSubs,
velSubs=velSubs,
doSimplify=doSimplify)