def linearise_matrix(M, x, x_f):
# reverse order of states to ensure velocities are subbed first
x_subs = {x[i]: x_f[i] for i in range(len(x))}
# get the value of M at the fixed point
M_f = me.msubs(M, x_subs)
# add a gradient term for each state about the fixed point
for i, xi in enumerate(x):
M_f += me.msubs(M.diff(xi), x_subs) * (xi - x_f[i])
return M_f
def msubs(self, *args):
"""
Creates a new instance of a Symbolic model with the substutions supplied
in args applied to all the Matricies
"""
ExtForces = self.ExtForces.msubs(
*args) if self.ExtForces is not None else None
# handle zero kinetic + pot energies
T = self.T if isinstance(self.T, int) else me.msubs(self.T, *args)
U = self.U if isinstance(self.U, int) else me.msubs(self.U, *args)
return SymbolicModel(me.msubs(self.M, *args), me.msubs(self.f, *args),
T, U, ExtForces)
def test_msubs():
a, b = symbols('a, b')
x, y, z = dynamicsymbols('x, y, z')
# Test simple substitution
expr = Matrix([[a * x + b, x * y.diff() + y],
[x.diff().diff(), z + sin(z.diff())]])
sol = Matrix([[a + b, y], [x.diff().diff(), 1]])
sd = {x: 1, z: 1, z.diff(): 0, y.diff(): 0}
assert msubs(expr, sd) == sol
# Test smart substitution
expr = cos(x + y) * tan(x + y) + b * x.diff()
sd = {x: 0, y: pi / 2, x.diff(): 1}
assert msubs(expr, sd, smart=True) == b + 1
def test_msubs():
a, b = symbols('a, b')
x, y, z = dynamicsymbols('x, y, z')
# Test simple substitution
expr = Matrix([[a*x + b, x*y.diff() + y],
[x.diff().diff(), z + sin(z.diff())]])
sol = Matrix([[a + b, y],
[x.diff().diff(), 1]])
sd = {x: 1, z: 1, z.diff(): 0, y.diff(): 0}
assert msubs(expr, sd) == sol
# Test smart substitution
expr = cos(x + y)*tan(x + y) + b*x.diff()
sd = {x: 0, y: pi/2, x.diff(): 1}
assert msubs(expr, sd, smart=True) == b + 1
def create_linear_equation(nl_expr):
linearized_expr = []
# for k in range(len(self.dstates)):
for k in range(len(nl_expr)):
linearized_term = 0
for j in range(len(self.states)):
linearized_term += msubs(diff(nl_expr[k], self.states[j]), substitutions_states) * (self.states[j] - substitutions_states[self.states[j]])
if working_point_inputs is not None:
for l in range(len(self.inputs)):
linearized_term += msubs(diff(nl_expr[k], self.inputs[l]), substitutions_states) * (self.inputs[l] - substitutions_states[self.inputs[l]])
linearized_term += msubs(nl_expr[k], substitutions_states)
linearized_expr.append(linearized_term)
return Array(linearized_expr)
def parallel(self, sys_append):
"""
A system is generated which is the result of a parallel connection of two systems. The inputs of this object are connected to the system that is placed in parallel and a new system is achieved with the output the sum of the outputs of both systems in parallel. Notice that the dimensions of the inputs and the outputs of both systems should be equal.
Parameters
-----------
sys_append : SystemBase object
the system that is added in parallel.
Returns
--------
A SystemBase object with the parallel system's equations.
Examples
---------
* Place 'sys2' in parallel with 'sys1' and show the inputs, states, state equations and output equations:
>>> parallel_sys = sys1.parallel(sys2)
>>> print('inputs: ', parallel_sys.system.input_)
>>> print('States: ', parallel_sys.system.state)
>>> print('State eqs: ', parallel_sys.system.state_equation)
>>> print('Output eqs: ', parallel_sys.system.output_equation)
"""
if (self.sys.dim_input != sys_append.sys.dim_input):
error_text = '[SystemBase.parallel] Dimension of the input of the first system is not equal to the dimension of the input of the second system.'
raise ValueError(error_text)
elif (self.sys.dim_output != sys_append.sys.dim_output):
error_text = '[SystemBase.parallel] Dimension of the output of the first system is not equal to the dimension of the output of the second system.'
raise ValueError(error_text)
else:
inputs = self.inputs
substitutions = dict(zip(sys_append.sys.input, self.sys.input))
output_equations = Array([value[0] + value[1] for value in zip(self.sys.output_equation, [msubs(expr, substitutions) for expr in sys_append.sys.output_equation])])
if (self.states is None):
if (sys_append.states is None):
return SystemBase(None, inputs, MemorylessSystem(input_=inputs, output_equation=output_equations))
else:
states = sys_append.states
state_equations = Array([msubs(expr, substitutions) for expr in sys_append.sys.state_equation])
return SystemBase(states, inputs, DynamicalSystem(state_equation=state_equations, state=states, input_=inputs, output_equation=output_equations))
else:
if (sys_append.states is None):
states = self.states
state_equations = self.sys.state_equation
else:
states = Array(self.states.tolist() + sys_append.states.tolist())
state_equations2 = Array(msubs(expr, substitutions) for expr in sys_append.sys.state_equation)
state_equations = Array(self.sys.state_equation.tolist() + state_equations2.tolist())
return SystemBase(states, inputs, DynamicalSystem(state_equation=state_equations, state=states, input_=inputs, output_equation=output_equations))
def linearize_system(n,kane):
"""
Take the previously derived equations of motion and create an LTI model
Inputs:
n - number of elements
kane - full nonlinear equations of motion
p - packed parameters (must have values)
Outputs:
A_np - Linearized A matrix as a numpy array
B_np - Linearized B matrix as a numpy array
"""
# Unpack the kanes method parameters
KM, fr, fr_star, q, u, Torque, F_in, lengths, masses = kane
# Linearize the Kane's method equations
linearizer = KM.to_linearizer()
# Output the A, B, and Mass matrices
Maz, A, B = linearizer.linearize()
# Create an operating point around which we will linearize
op_point = dict()
# we will linearize about the undeflected, stationary point
for h in range(n):
op_point[q[h]] = 0.0
op_point[u[h]] = 0.0
# Perform substitutions to solve for the linearized matrices
M_op = me.msubs(Maz, op_point)
A_op = me.msubs(A, op_point)
B_op = me.msubs(B, op_point)
perm_mat = linearizer.perm_mat
# Solve for the linear A and B matrices
A_lin = perm_mat.T * M_op.LUsolve(A_op)
B_lin = perm_mat.T * M_op.LUsolve(B_op)
A_sol = A_lin.subs(op_point).doit()
B_sol = B_lin.subs(op_point).doit()
# Ensure the matrices are of the correct data type
A_np = np.array(np.array(A_sol), np.float)
B_np = np.array(np.array(B_sol), np.float)
return A_np,B_np
def create_state_equations(self):
"""
As the system contains a second derivative of the states, an extended state should be used, which contains the first derivative of the states as well. Therefore, the state equation has to be adapted to this new state vector.
Returns
--------
result : sympy array object
the state equation for each element in self.states
"""
minimal_dstates = Matrix(self.states[1::2])
dstates = Matrix(self.dstates[0::2])
substitution = dict(zip(dstates, minimal_dstates))
M_inv = self.inertia_matrix.inv()
states_dotdot = M_inv * self.force_vector \
- M_inv * self.damping_matrix * minimal_dstates \
- M_inv * self.stiffness_matrix
states_dot = []
for i in range(len(self.states)):
if i % 2 == 0:
states_dot.append(self.states[i + 1])
else:
states_dot.append( \
msubs(states_dotdot[i//2].copy(), substitution))
states_dot = Array(states_dot)
return states_dot
def _lambdify(self, outputs):
# TODO : We could forgo this substitution for generation speed
# purposes and have lots of args for lambdify (like it used to be
# done) but there may be some limitations on number of args.
subs = {}
vec_inputs = []
if self.specifieds is None:
def_vecs = ['q', 'u', 'p']
else:
def_vecs = ['q', 'u', 'r', 'p']
for syms, vec_name in zip(self.inputs, def_vecs):
v = sm.DeferredVector(vec_name)
for i, sym in enumerate(syms):
subs[sym] = v[i]
vec_inputs.append(v)
try:
outputs = [me.msubs(output, subs) for output in outputs]
except AttributeError:
# msubs doesn't exist in SymPy < 0.7.6.
outputs = [output.subs(subs) for output in outputs]
modules = [{'ImmutableMatrix': np.array}, 'numpy']
return sm.lambdify(vec_inputs, outputs, modules=modules)
def _lambdify(self, outputs):
# TODO : We could forgo this substitution for generation speed
# purposes and have lots of args for lambdify (like it used to be
# done) but there may be some limitations on number of args.
subs = {}
vec_inputs = []
if self.specifieds is None:
def_vecs = ['q', 'u', 'p']
else:
def_vecs = ['q', 'u', 'r', 'p']
for syms, vec_name in zip(self.inputs, def_vecs):
v = sm.DeferredVector(vec_name)
for i, sym in enumerate(syms):
subs[sym] = v[i]
vec_inputs.append(v)
try:
outputs = [me.msubs(output, subs) for output in outputs]
except AttributeError:
# msubs doesn't exist in SymPy < 0.7.6.
outputs = [output.subs(subs) for output in outputs]
modules = [{'ImmutableMatrix': np.array}, 'numpy']
return sm.lambdify(vec_inputs, outputs, modules=modules)
def U_gen(i):
U_local = U_expr[i].doit()
local_list = []
for j in q_func:
rs = diff(U_local, j)
local_list.append(msubs(rs, coordinate_subs))
print(f'Now generating {i+1}/{len(U_expr)} term of U')
return local_list
def T_gen(i):
T_local = T_expr[i].doit()
local_list = []
for j in q_func_dt:
rs = diff(diff(T_local, j), t)
local_list.append(msubs(rs, coordinate_subs))
print(f'Now generating {i+1}/{len(T_expr)} term of T')
return local_list
def series(self, sys_append):
"""
A system is generated which is the result of a serial connection of two systems. The outputs of this object are connected to the inputs of the appended system and a new system is achieved which has the inputs of the current system and the outputs of the appended system. Notice that the dimensions of the output of the current system should be equal to the dimension of the input of the appended system.
Parameters
-----------
sys_append : SystemBase object
the system that is placed in a serial configuration. 'sys_append' follows the current system.
Returns
--------
A SystemBase object with the serial system's equations.
Examples
---------
* Place 'sys1' behind 'sys2' in a serial configuration and show the inputs, states, state equations and output equations:
>>> series_sys = sys1.series(sys2)
>>> print('inputs: ', series_sys.system.input_)
>>> print('States: ', series_sys.system.state)
>>> print('State eqs: ', series_sys.system.state_equation)
>>> print('Output eqs: ', series_sys.system.output_equation)
"""
if (self.sys.dim_output != sys_append.sys.dim_input):
error_text = '[SystemBase.series] Dimension of output of the first system is not equal to dimension of input of the second system.'
raise ValueError(error_text)
# raise SystemExit(error_text), None, sys.exc_info()[2]
else:
inputs = self.inputs
substitutions = dict(zip(sys_append.sys.input, self.sys.output_equation))
output_equations = Array([msubs(expr, substitutions) for expr in sys_append.sys.output_equation])
if (self.states is None):
if (sys_append.states is None):
return SystemBase(None, inputs, MemorylessSystem(input_=inputs, output_equation=output_equations))
else:
states = sys_append.states
state_equations = Array([msubs(expr, substitutions) for expr in sys_append.sys.state_equation])
return SystemBase(states, inputs, DynamicalSystem(state_equation=state_equations, state=states, input_=inputs, output_equation=output_equations))
else:
if (sys_append.states is None):
states = self.states
state_equations = self.sys.state_equation
else:
states = Array(self.states.tolist() + sys_append.states.tolist())
state_equations2 = Array(msubs(expr, substitutions) for expr in sys_append.sys.state_equation)
state_equations = Array(self.sys.state_equation.tolist() + state_equations2.tolist())
return SystemBase(states, inputs, DynamicalSystem(state_equation=state_equations, state=states, input_=inputs, output_equation=output_equations))
def U_gen(i):
U_local = sympify(U_strings[i])
local_list = []
for j in q_func:
rs = diff(U_local, j)
local_list.append(msubs(rs, coordinate_subs))
print(f'Now generating {i+1}/{len(un)} term of U')
return local_list
def clean(x, expr, *sub_dicts):
"""Clean up an expression, to allow for fast simulation"""
xvec = MatrixSymbol('x', len(x), 1)
xdict = dict(zip(x, xvec))
sub_dicts = sub_dicts + (xdict,)
expr = msubs(expr, *sub_dicts)
return xvec, expr
def T_gen(i):
T_local = sympify(T_strings[i])
local_list = []
for j in q_func_dt:
rs = diff(diff(T_local, j), t)
local_list.append(msubs(rs, coordinate_subs))
print(f'Now generating {i+1}/{len(tn)} term of T')
return local_list
def insertQQd(expr, dofs):
#syms = [phi_x(t), phi_y(t), phi_z(t)]
# See pydy.codegen.ode_function_generator _lambdaify
subs = {}
vec_name = 'q'
q = DeferredVector('q')
qd = DeferredVector('qd')
for i, sym in enumerate(dofs):
dsym = diff(sym, dynamicsymbols._t)
subs[sym] = q[i]
subs[dsym] = qd[i]
print(subs)
if hasattr(expr, '__len__'):
print(expr.shape)
return Matrix([me.msubs(e, subs) for e in expr]).reshape(expr.shape[0],expr.shape[1])
else:
return me.msubs(expr, subs)
def __get_states__(self):
'''
Contcatenate the states vector of the system and the controller.
Returns
--------
states : list
first the states of the system and next the states of the controller.
state_equations : list
first the state equations of the system and next the state equations of the controller.
'''
states = []
state_equations = []
if self.system is None:
if self.controller is None:
error_text = '[ClosedLoop.__get_states__] Both controller and system are None. One of them should at least contain a system.'
raise AssertionError(error_text)
else:
if self.controller.states is not None:
states.extend(self.controller.states)
state_equations.extend(self.controller.state_equation)
else:
substitutions_derivatives = dict()
unprocessed_substitutions_system = zip(self.system.inputs, (-1) * self.controller.output_equation)
if self.system.states is not None:
# Remove Derivative(., 't') from controller states and substitutions_system
minimal_dstates = self.system.states[1::2]
dstates = self.system.dstates[0::2]
substitutions_derivatives = dict(zip(dstates, minimal_dstates))
substitutions_system = dict([(k, msubs(v, substitutions_derivatives))\
for k, v in unprocessed_substitutions_system])
states.extend(self.system.states)
state_equations.extend([msubs(state_eq, substitutions_derivatives, substitutions_system)\
for state_eq in self.system.state_equation])
if self.controller.states is not None:
states.extend(self.controller.states)
controller_state_eq = msubs(self.controller.state_equation, substitutions_derivatives)
state_equations.extend(controller_state_eq)
return states, state_equations
def test_msubs():
a, b = symbols("a, b")
x, y, z = dynamicsymbols("x, y, z")
# Test simple substitution
expr = Matrix([[a * x + b, x * y.diff() + y], [x.diff().diff(), z + sin(z.diff())]])
sol = Matrix([[a + b, y], [x.diff().diff(), 1]])
sd = {x: 1, z: 1, z.diff(): 0, y.diff(): 0}
assert msubs(expr, sd) == sol
# Test smart substitution
expr = cos(x + y) * tan(x + y) + b * x.diff()
sd = {x: 0, y: pi / 2, x.diff(): 1}
assert msubs(expr, sd, smart=True) == b + 1
N = ReferenceFrame("N")
v = x * N.x + y * N.y
d = x * (N.x | N.x) + y * (N.y | N.y)
v_sol = 1 * N.y
d_sol = 1 * (N.y | N.y)
sd = {x: 0, y: 1}
assert msubs(v, sd) == v_sol
assert msubs(d, sd) == d_sol
def test_msubs():
a, b = symbols('a, b')
x, y, z = dynamicsymbols('x, y, z')
# Test simple substitution
expr = Matrix([[a * x + b, x * y.diff() + y],
[x.diff().diff(), z + sin(z.diff())]])
sol = Matrix([[a + b, y], [x.diff().diff(), 1]])
sd = {x: 1, z: 1, z.diff(): 0, y.diff(): 0}
assert msubs(expr, sd) == sol
# Test smart substitution
expr = cos(x + y) * tan(x + y) + b * x.diff()
sd = {x: 0, y: pi / 2, x.diff(): 1}
assert msubs(expr, sd, smart=True) == b + 1
N = ReferenceFrame('N')
v = x * N.x + y * N.y
d = x * (N.x | N.x) + y * (N.y | N.y)
v_sol = 1 * N.y
d_sol = 1 * (N.y | N.y)
sd = {x: 0, y: 1}
assert msubs(v, sd) == v_sol
assert msubs(d, sd) == d_sol
def __init__(self, rhs, dynamics, params=[], values=[]):
self.rhs = rhs
self.dynamics = dynamics
self.params = params
self.values = values
# substitute dummy variables into the equations of motion for later evaluation
self._dummys = [Dummy() for i in self.dynamics]
self._dummy_dict = dict(zip(self.dynamics, self._dummys))
self._dummy_rhs = msubs(self.rhs, self._dummy_dict)
# create a function to evaluate the right hand side
self._rhs_func = lambdify(
list(self._dummys) + list(self.params), self._dummy_rhs)
def _discretize_eom(self):
"""Instantiates the constraint equations in a discretized form using
backward Euler discretization.
Instantiates
------------
discrete_eoms : sympy.Matrix, shape(n, 1)
The column vector of the discretized equations of motion.
"""
x = self.state_symbols
xd = self.state_derivative_symbols
u = self.input_trajectories
xp = self.previous_discrete_state_symbols
xi = self.current_discrete_state_symbols
xn = self.next_discrete_state_symbols
ui = self.current_discrete_specified_symbols
un = self.next_discrete_specified_symbols
h = self.time_interval_symbol
if self.integration_method == 'backward euler':
deriv_sub = {d: (i - p) / h for d, i, p in zip(xd, xi, xp)}
func_sub = dict(zip(x + u, xi + ui))
self.discrete_eom = me.msubs(self.eom, deriv_sub, func_sub)
elif self.integration_method == 'midpoint':
xdot_sub = {d: (n - i) / h for d, i, n in zip(xd, xi, xn)}
x_sub = {d: (i + n) / 2 for d, i, n in zip(x, xi, xn)}
u_sub = {d: (i + n) / 2 for d, i, n in zip(u, ui, un)}
self.discrete_eom = me.msubs(self.eom, xdot_sub, x_sub, u_sub)
def gen_lin_eigen_problem(self, p):
"""
gets the genralised eigan matrices for use in solving the frequencies / modes.
They are of the form:
| I 0 | | 0 I |
M= | 0 M | ,K= | E-C D-B |
such that scipy.linalg.eig(K,M) solves the problem
THE SYSTEM MUST BE LINEARISED FOR THIS TO WORK
"""
x = [j for i in range(len(p.q)) for j in [p.q[i], p.qd[i], p.qdd[i]]]
fp = [
j for i in range(len(p.q))
for j in [p.fp[::2][i], p.fp[1::2][i], 0]
]
x_subs = {x[i]: fp[i] for i in range(len(x))}
_Q = self.ExtForces.Q() if self.ExtForces is not None else sym.Matrix(
[0] * p.qs)
f = (_Q - self.f)
A = me.msubs(self.M, x_subs)
B = me.msubs(self._jacobian(f, p.qd), x_subs)
C = me.msubs(self._jacobian(f, p.q), x_subs)
M_prime = sym.eye(p.qs * 2)
M_prime[-p.qs:, -p.qs:] = A
K_prime = sym.zeros(p.qs * 2)
K_prime[:p.qs, -p.qs:] = sym.eye(p.qs)
K_prime[-p.qs:, :p.qs] = C
K_prime[-p.qs:, -p.qs:] = B
return K_prime, M_prime
def _gen_octave(self, expr, p, func_name):
# convert states to non-time dependent variable
U = sym.Matrix(sym.symbols(f'u_:{p.qs*2}'))
l = dict(zip(p.x, U))
expr = me.msubs(expr, l)
# convert to octave string and covert states to vector form
out = sym.printing.octave.octave_code(expr)
my_replace = lambda x: f'U({int(x.group(1))+1})'
out = re.sub(r"u_(?P<index>\d+)", my_replace, out)
# make the file pretty...
out = out.replace(',', ',...\n\t\t').replace(';', ';...\n\t\t')
# wrap output in octave function signature
signature = f'function out = {func_name}(U,p)'
octave_string = signature + '\n\t' + 'out = ' + out + ';\nend'
return octave_string
def form_newton_euler_equations(self,ForceList,TorqueList,Body,NewtonBasis,EulerBasis,dictionary): """Generate Newton-Euler equations, sets the equations attribute and returns the equations Arguments ========= ForceList: iterable of 2-tuples (Point actionPoint, Vector force) actionPoint is where the force is applied, force, a vector describing the magnitude and direction of the force TODO: create a force class TorqueList: iterable of Vectors each member is a Torgue acting on the Body TODO: create a torque class? something about moments and transfering points, look into torque/force couples Body: RigidBody Body to apply Newton-Euler to TODO: implement list of bodies instead of single body EquationFrame: 2-tuple (ReferenceFrame NewtonFrame, ReferenceFrame EulerFrame) NewtonFrame: a ReferenceFrame to generate the Newton equations in EulerFrame: a ReferenceFrame to generate the Euler equations in TODO: use a vector basis instead of frames. consider splitting into two arguments for each body: newtonbasis, eulerbasis handle non-right handed or orthogonal bases handle a basis whose member leads to an indeterminant equation dictionary: dictionary to clean up ODE (clean variables:ugly derivatives) TODO: to be removed and generated internally """ self.newtonbasis = NewtonBasis self.eulerbasis = EulerBasis self.body=Body center=self.body.masscenter NetForce = sum([force for point, force in ForceList]) NetTorque = sum(TorqueList) NetMoment = NetTorque + sum([point.pos_from(center).cross(force) for point, force in ForceList]) NewtonEquation=NetForce-self.body.mass*center.acc(self.inertialframe) EulerEquation=NetMoment-self.body.inertia[0].dot(self.body.frame.ang_acc_in(self.inertialframe)) newtondiffeq = [NewtonEquation.dot(basisvector) for basisvector in self.newtonbasis] eulerdiffeq = [EulerEquation.dot(basisvector) for basisvector in self.eulerbasis] diffeq = newtondiffeq + eulerdiffeq #diffeq=[NewtonEquation.dot(self.newtonframe.x),NewtonEquation.dot(self.newtonframe.y),NewtonEquation.dot(self.newtonframe.z),EulerEquation.dot(self.eulerframe.x),EulerEquation.dot(self.eulerframe.y),EulerEquation.dot(self.eulerframe.z)] self.equations=[msubs(eqn,dictionary) for eqn in diffeq] return self.equations
def GenRectWingModel(b_modes,
t_modes,
fwt_free,
iwt,
iwb,
fwt_Iyy=False,
fwt_ke_simp=False,
aero_model=AeroModel.LiftOnly):
p = rw.base_params(b_modes + t_modes + 1)
#get shape functions for main wing
z_0, tau_0 = ma.ShapeFunctions_BN_TM(b_modes,
t_modes,
p.q[:-1],
p.y_0,
p.x_0,
p.x_f0,
0,
factor=p.eta)
#define wrefernce frames
wing_root_frame = ma.HomogenousTransform().R_y(p.alpha_r)
wing_frame = wing_root_frame.Translate(p.x_0, p.y_0, z_0)
wing_flexural_frame = wing_frame.msubs({p.x_0: p.x_f0})
fwt_root_frame = wing_frame.msubs({
p.y_0: p.s_0,
p.x_0: p.x_f0
}).Translate(-p.x_f0, 0, 0)
if fwt_free:
fwt_root_frame = fwt_root_frame.R_x(-p.q[-1])
fwt_flexural_frame = fwt_root_frame.Translate(p.x_f1, p.y_1, 0)
fwt_com_frame = fwt_root_frame.Translate(p.c / 2, p.s_1 / 2, 0)
#Create Elemnts
M_wing = ele.MassMatrix(p.rho_t)
inner_wing_ele = ele.FlexiElement(wing_root_frame,
M_wing,
p.x_0,
p.y_0,
z_0,
p.c,
p.s_0,
p.x_f0,
p.EI,
p.GJ,
gravityPot=True)
I_yy = 0
if fwt_Iyy:
I_yy += sym.Rational(1, 2) * p.m_1 * p.c + p.m_1 * (p.c / 2)**2
if fwt_ke_simp:
M_fwt = ele.MassMatrix(p.m_1,
I_xx=p.I_xx_1 + p.m_1 * (p.s_1 / 2)**2,
I_yy=I_yy)
fwt_ele = ele.RigidElement(fwt_root_frame,
M_fwt,
True,
com_pos=[0, p.s_1 / 2, 0])
else:
M_fwt = ele.MassMatrix(p.m_1, I_xx=p.I_xx_1, I_yy=I_yy)
fwt_ele = ele.RigidElement(fwt_com_frame, M_fwt, True)
# Create AeroForces
wing_AeroForces = ef.AeroForce_1.PerUnitSpan(
p,
wing_flexural_frame,
p.a_0,
alphadot=tau_0 if isinstance(tau_0, int) else tau_0.diff(t),
M_thetadot=p.M_thetadot,
e=p.e_0,
rootAlpha=p.alpha_r,
deltaAlpha=tau_0,
alpha_zero=0).integrate((p.y_0, 0, p.s_0))
alpha_fwt = 0
alphadot_fwt = 0
if fwt_free:
alpha_fwt += p.alpha_1
alphadot_fwt += p.alphadot_1
if aero_model == AeroModel.LiftOnly:
fwt_AeroForces_perUnit = ef.AeroForce_1.PerUnitSpan(
p,
fwt_flexural_frame,
p.a_1,
alphadot=p.alphadot_1,
M_thetadot=p.M_thetadot,
e=p.e_1,
rootAlpha=p.alpha_1,
deltaAlpha=0,
alpha_zero=0)
elif aero_model == AeroModel.LiftOnly_rot:
fwt_AeroForces_perUnit = ef.AeroForce_2.PerUnitSpan(
p,
fwt_flexural_frame,
p.a_1,
alphadot=p.alphadot_1,
M_thetadot=p.M_thetadot,
e=p.e_1,
rootAlpha=p.alpha_1,
alpha_zero=0,
include_drag=False)
else:
fwt_AeroForces_perUnit = ef.AeroForce_3.PerUnitSpan(
p,
fwt_flexural_frame,
p.a_1,
alphadot=p.alphadot_1,
M_thetadot=p.M_thetadot,
e=p.e_1,
rootAlpha=p.alpha_1,
alpha_zero=0,
stall_angle=p.alpha_s,
c_d_max=p.c_dmax)
forces = []
segments = 5
for i in range(segments):
seg_width = p.s_1 / segments
yi = seg_width / 2 + i * seg_width
forces.append(fwt_AeroForces_perUnit.subs({p.y_1: yi}) * seg_width)
Q = sym.Matrix([0] * p.qs)
for f in forces:
Q += f.Q()
fwt_AeroForces = ef.ExternalForce(Q)
# Setup AoA of FWT
fwt_aoa = ma.GetAoA(p.alpha_r, 0, p.Lambda, 0 if not fwt_free else p.q[-1])
if iwb:
wing_bend = sym.atan(
z_0.diff(p.y_0).subs({
p.x_0: p.x_f0,
p.y_0: p.s_0
}))
fwt_aoa = me.msubs(fwt_aoa, {p.q[-1]: p.q[-1] - wing_bend})
if iwt:
tau_s0 = tau_0.subs(p.y_0, p.s_0)
fwt_aoa = me.msubs(fwt_aoa, {p.alpha_r: p.alpha_r + tau_s0})
## Sub in Aero Forces
fwt_AeroForces = fwt_AeroForces.subs({
p.alpha_1: fwt_aoa,
p.alphadot_1: fwt_aoa.diff(t)
})
#Create Composite force
CompositeForce = ef.CompositeForce([wing_AeroForces, fwt_AeroForces])
# Create the SYmbolic Model
sm = ma.SymbolicModel.FromElementsAndForces(p, [inner_wing_ele, fwt_ele],
CompositeForce)
return sm, p
def eigen_perm_params(p,
model,
vars_ls,
calc_fixed_points,
jac=True,
fixed_point_gen=None,
guess_v0=None,
fp=None,
sortby='F'):
"""
Method to generate the flutter results for a model for each permutation of the parms in param_perms:
p - instance of Model Parameters
model - instance of symbolic model using params defined in p
param_perms - a list of tuples, each tuple consists of (Symbol, list of values)
calc_fixed_points - if True, will calc the fixed point for each permutation
"""
if fp is None:
fp = [0] * p.qs
#get list of symbols to key in model
variables = [k for k, v in vars_ls]
model_mini = model.msubs(p.GetSubs(0, p.fp, ignore=variables))
# get eigen Matrices and turn into a function
K, M = model_mini.gen_lin_eigen_problem(p)
#get free body problem
K_v0 = msubs(K, {p.V: 0})
M_v0 = msubs(M, {p.V: 0})
func = sym.lambdify((variables, p.fp), (K, M))
func_v0 = sym.lambdify((variables, p.fp), (K_v0, M_v0))
# If caluclating fixed points generate require objective functions
if calc_fixed_points:
if fixed_point_gen is None:
f = msubs((model_mini.f - model_mini.ExtForces.Q()),
{i: 0
for i in p.qd})
else:
f = fixed_point_gen(model_mini)
f_v0 = msubs(f, {p.V: 0})
func_obj = sym.lambdify((p.q, variables), f, "numpy")
func_obj_v0 = sym.lambdify((p.q, variables), f_v0, "numpy")
if jac:
func_jac_obj = sym.lambdify((p.q, variables), f.jacobian(p.q),
"numpy")
# Get all possible combinations of the variables
perms = np.array(np.meshgrid(*[v for k, v in vars_ls])).T.reshape(
-1, len(vars_ls))
df_perms = [[(vars_ls[i][0], row[i]) for i in range(len(row))]
for row in perms]
string_perms = [{vars_ls[i][0].name: row[i]
for i in range(len(row))} for row in perms]
#Calc freqs and dampings
flutdfv2 = []
qs = []
for i in range(len(df_perms)):
values = tuple([v for k, v in df_perms[i]])
# Calc fixed point
#set the initial guess (if v=0 set to FWT dropped doen else use previous result)
if calc_fixed_points:
if string_perms[i]["V"] == 0:
if guess_v0 is None:
guess = [0] * p.qs
guess[-1] = np.pi / 2
else:
guess = guess_v0
q = fsolve(lambda q, v: func_obj_v0(q, v)[:, 0],
guess,
factor=1,
args=(values, ))
else:
if i > 0:
guess = qs[i - 1]
else:
guess = [0] * p.qs
guess[-1] = 0.1
q = root(lambda q, v: func_obj(q, values)[:, 0],
guess,
jac=func_jac_obj if jac else None,
args=(values, )).x
else:
q = fp
qs.append(q)
x = [0] * p.qs * 2
x[::2] = q
if string_perms[i]["V"] == 0:
evals, evecs = eig(*func_v0(values, x))
else:
evals, evecs = eig(*func(values, x))
jac_dat = ma.extract_eigen_value_data(evals, evecs, sortby=sortby)
#create q and perm data to match size
for j in range(len(jac_dat)):
flutdfv2.append({**jac_dat[j], **string_perms[i], 'q': q})
return pd.DataFrame(flutdfv2)
def msubs(self, *args):
return AeroForce(me.msubs(self._Q, *args),
me.msubs(self.dAlpha, *args))
def test_bicycle():
if ON_TRAVIS:
skip("Too slow for travis.")
# Code to get equations of motion for a bicycle modeled as in:
# J.P Meijaard, Jim M Papadopoulos, Andy Ruina and A.L Schwab. Linearized
# dynamics equations for the balance and steer of a bicycle: a benchmark
# and review. Proceedings of The Royal Society (2007) 463, 1955-1982
# doi: 10.1098/rspa.2007.1857
# Note that this code has been crudely ported from Autolev, which is the
# reason for some of the unusual naming conventions. It was purposefully as
# similar as possible in order to aide debugging.
# Declare Coordinates & Speeds
# Simple definitions for qdots - qd = u
# Speeds are: yaw frame ang. rate, roll frame ang. rate, rear wheel frame
# ang. rate (spinning motion), frame ang. rate (pitching motion), steering
# frame ang. rate, and front wheel ang. rate (spinning motion).
# Wheel positions are ignorable coordinates, so they are not introduced.
q1, q2, q4, q5 = dynamicsymbols('q1 q2 q4 q5')
q1d, q2d, q4d, q5d = dynamicsymbols('q1 q2 q4 q5', 1)
u1, u2, u3, u4, u5, u6 = dynamicsymbols('u1 u2 u3 u4 u5 u6')
u1d, u2d, u3d, u4d, u5d, u6d = dynamicsymbols('u1 u2 u3 u4 u5 u6', 1)
# Declare System's Parameters
WFrad, WRrad, htangle, forkoffset = symbols('WFrad WRrad htangle forkoffset')
forklength, framelength, forkcg1 = symbols('forklength framelength forkcg1')
forkcg3, framecg1, framecg3, Iwr11 = symbols('forkcg3 framecg1 framecg3 Iwr11')
Iwr22, Iwf11, Iwf22, Iframe11 = symbols('Iwr22 Iwf11 Iwf22 Iframe11')
Iframe22, Iframe33, Iframe31, Ifork11 = symbols('Iframe22 Iframe33 Iframe31 Ifork11')
Ifork22, Ifork33, Ifork31, g = symbols('Ifork22 Ifork33 Ifork31 g')
mframe, mfork, mwf, mwr = symbols('mframe mfork mwf mwr')
# Set up reference frames for the system
# N - inertial
# Y - yaw
# R - roll
# WR - rear wheel, rotation angle is ignorable coordinate so not oriented
# Frame - bicycle frame
# TempFrame - statically rotated frame for easier reference inertia definition
# Fork - bicycle fork
# TempFork - statically rotated frame for easier reference inertia definition
# WF - front wheel, again posses a ignorable coordinate
N = ReferenceFrame('N')
Y = N.orientnew('Y', 'Axis', [q1, N.z])
R = Y.orientnew('R', 'Axis', [q2, Y.x])
Frame = R.orientnew('Frame', 'Axis', [q4 + htangle, R.y])
WR = ReferenceFrame('WR')
TempFrame = Frame.orientnew('TempFrame', 'Axis', [-htangle, Frame.y])
Fork = Frame.orientnew('Fork', 'Axis', [q5, Frame.x])
TempFork = Fork.orientnew('TempFork', 'Axis', [-htangle, Fork.y])
WF = ReferenceFrame('WF')
# Kinematics of the Bicycle First block of code is forming the positions of
# the relevant points
# rear wheel contact -> rear wheel mass center -> frame mass center +
# frame/fork connection -> fork mass center + front wheel mass center ->
# front wheel contact point
WR_cont = Point('WR_cont')
WR_mc = WR_cont.locatenew('WR_mc', WRrad * R.z)
Steer = WR_mc.locatenew('Steer', framelength * Frame.z)
Frame_mc = WR_mc.locatenew('Frame_mc', - framecg1 * Frame.x
+ framecg3 * Frame.z)
Fork_mc = Steer.locatenew('Fork_mc', - forkcg1 * Fork.x
+ forkcg3 * Fork.z)
WF_mc = Steer.locatenew('WF_mc', forklength * Fork.x + forkoffset * Fork.z)
WF_cont = WF_mc.locatenew('WF_cont', WFrad * (dot(Fork.y, Y.z) * Fork.y -
Y.z).normalize())
# Set the angular velocity of each frame.
# Angular accelerations end up being calculated automatically by
# differentiating the angular velocities when first needed.
# u1 is yaw rate
# u2 is roll rate
# u3 is rear wheel rate
# u4 is frame pitch rate
# u5 is fork steer rate
# u6 is front wheel rate
Y.set_ang_vel(N, u1 * Y.z)
R.set_ang_vel(Y, u2 * R.x)
WR.set_ang_vel(Frame, u3 * Frame.y)
Frame.set_ang_vel(R, u4 * Frame.y)
Fork.set_ang_vel(Frame, u5 * Fork.x)
WF.set_ang_vel(Fork, u6 * Fork.y)
# Form the velocities of the previously defined points, using the 2 - point
# theorem (written out by hand here). Accelerations again are calculated
# automatically when first needed.
WR_cont.set_vel(N, 0)
WR_mc.v2pt_theory(WR_cont, N, WR)
Steer.v2pt_theory(WR_mc, N, Frame)
Frame_mc.v2pt_theory(WR_mc, N, Frame)
Fork_mc.v2pt_theory(Steer, N, Fork)
WF_mc.v2pt_theory(Steer, N, Fork)
WF_cont.v2pt_theory(WF_mc, N, WF)
# Sets the inertias of each body. Uses the inertia frame to construct the
# inertia dyadics. Wheel inertias are only defined by principle moments of
# inertia, and are in fact constant in the frame and fork reference frames;
# it is for this reason that the orientations of the wheels does not need
# to be defined. The frame and fork inertias are defined in the 'Temp'
# frames which are fixed to the appropriate body frames; this is to allow
# easier input of the reference values of the benchmark paper. Note that
# due to slightly different orientations, the products of inertia need to
# have their signs flipped; this is done later when entering the numerical
# value.
Frame_I = (inertia(TempFrame, Iframe11, Iframe22, Iframe33, 0, 0, Iframe31), Frame_mc)
Fork_I = (inertia(TempFork, Ifork11, Ifork22, Ifork33, 0, 0, Ifork31), Fork_mc)
WR_I = (inertia(Frame, Iwr11, Iwr22, Iwr11), WR_mc)
WF_I = (inertia(Fork, Iwf11, Iwf22, Iwf11), WF_mc)
# Declaration of the RigidBody containers. ::
BodyFrame = RigidBody('BodyFrame', Frame_mc, Frame, mframe, Frame_I)
BodyFork = RigidBody('BodyFork', Fork_mc, Fork, mfork, Fork_I)
BodyWR = RigidBody('BodyWR', WR_mc, WR, mwr, WR_I)
BodyWF = RigidBody('BodyWF', WF_mc, WF, mwf, WF_I)
# The kinematic differential equations; they are defined quite simply. Each
# entry in this list is equal to zero.
kd = [q1d - u1, q2d - u2, q4d - u4, q5d - u5]
# The nonholonomic constraints are the velocity of the front wheel contact
# point dotted into the X, Y, and Z directions; the yaw frame is used as it
# is "closer" to the front wheel (1 less DCM connecting them). These
# constraints force the velocity of the front wheel contact point to be 0
# in the inertial frame; the X and Y direction constraints enforce a
# "no-slip" condition, and the Z direction constraint forces the front
# wheel contact point to not move away from the ground frame, essentially
# replicating the holonomic constraint which does not allow the frame pitch
# to change in an invalid fashion.
conlist_speed = [WF_cont.vel(N) & Y.x, WF_cont.vel(N) & Y.y, WF_cont.vel(N) & Y.z]
# The holonomic constraint is that the position from the rear wheel contact
# point to the front wheel contact point when dotted into the
# normal-to-ground plane direction must be zero; effectively that the front
# and rear wheel contact points are always touching the ground plane. This
# is actually not part of the dynamic equations, but instead is necessary
# for the lineraization process.
conlist_coord = [WF_cont.pos_from(WR_cont) & Y.z]
# The force list; each body has the appropriate gravitational force applied
# at its mass center.
FL = [(Frame_mc, -mframe * g * Y.z),
(Fork_mc, -mfork * g * Y.z),
(WF_mc, -mwf * g * Y.z),
(WR_mc, -mwr * g * Y.z)]
BL = [BodyFrame, BodyFork, BodyWR, BodyWF]
# The N frame is the inertial frame, coordinates are supplied in the order
# of independent, dependent coordinates, as are the speeds. The kinematic
# differential equation are also entered here. Here the dependent speeds
# are specified, in the same order they were provided in earlier, along
# with the non-holonomic constraints. The dependent coordinate is also
# provided, with the holonomic constraint. Again, this is only provided
# for the linearization process.
KM = KanesMethod(N, q_ind=[q1, q2, q5],
q_dependent=[q4], configuration_constraints=conlist_coord,
u_ind=[u2, u3, u5],
u_dependent=[u1, u4, u6], velocity_constraints=conlist_speed,
kd_eqs=kd)
(fr, frstar) = KM.kanes_equations(FL, BL)
# This is the start of entering in the numerical values from the benchmark
# paper to validate the eigen values of the linearized equations from this
# model to the reference eigen values. Look at the aforementioned paper for
# more information. Some of these are intermediate values, used to
# transform values from the paper into the coordinate systems used in this
# model.
PaperRadRear = 0.3
PaperRadFront = 0.35
HTA = evalf.N(pi / 2 - pi / 10)
TrailPaper = 0.08
rake = evalf.N(-(TrailPaper*sin(HTA)-(PaperRadFront*cos(HTA))))
PaperWb = 1.02
PaperFrameCgX = 0.3
PaperFrameCgZ = 0.9
PaperForkCgX = 0.9
PaperForkCgZ = 0.7
FrameLength = evalf.N(PaperWb*sin(HTA)-(rake-(PaperRadFront-PaperRadRear)*cos(HTA)))
FrameCGNorm = evalf.N((PaperFrameCgZ - PaperRadRear-(PaperFrameCgX/sin(HTA))*cos(HTA))*sin(HTA))
FrameCGPar = evalf.N((PaperFrameCgX / sin(HTA) + (PaperFrameCgZ - PaperRadRear - PaperFrameCgX / sin(HTA) * cos(HTA)) * cos(HTA)))
tempa = evalf.N((PaperForkCgZ - PaperRadFront))
tempb = evalf.N((PaperWb-PaperForkCgX))
tempc = evalf.N(sqrt(tempa**2+tempb**2))
PaperForkL = evalf.N((PaperWb*cos(HTA)-(PaperRadFront-PaperRadRear)*sin(HTA)))
ForkCGNorm = evalf.N(rake+(tempc * sin(pi/2-HTA-acos(tempa/tempc))))
ForkCGPar = evalf.N(tempc * cos((pi/2-HTA)-acos(tempa/tempc))-PaperForkL)
# Here is the final assembly of the numerical values. The symbol 'v' is the
# forward speed of the bicycle (a concept which only makes sense in the
# upright, static equilibrium case?). These are in a dictionary which will
# later be substituted in. Again the sign on the *product* of inertia
# values is flipped here, due to different orientations of coordinate
# systems.
v = symbols('v')
val_dict = {WFrad: PaperRadFront,
WRrad: PaperRadRear,
htangle: HTA,
forkoffset: rake,
forklength: PaperForkL,
framelength: FrameLength,
forkcg1: ForkCGPar,
forkcg3: ForkCGNorm,
framecg1: FrameCGNorm,
framecg3: FrameCGPar,
Iwr11: 0.0603,
Iwr22: 0.12,
Iwf11: 0.1405,
Iwf22: 0.28,
Ifork11: 0.05892,
Ifork22: 0.06,
Ifork33: 0.00708,
Ifork31: 0.00756,
Iframe11: 9.2,
Iframe22: 11,
Iframe33: 2.8,
Iframe31: -2.4,
mfork: 4,
mframe: 85,
mwf: 3,
mwr: 2,
g: 9.81,
q1: 0,
q2: 0,
q4: 0,
q5: 0,
u1: 0,
u2: 0,
u3: v / PaperRadRear,
u4: 0,
u5: 0,
u6: v / PaperRadFront}
# Linearizes the forcing vector; the equations are set up as MM udot =
# forcing, where MM is the mass matrix, udot is the vector representing the
# time derivatives of the generalized speeds, and forcing is a vector which
# contains both external forcing terms and internal forcing terms, such as
# centripital or coriolis forces. This actually returns a matrix with as
# many rows as *total* coordinates and speeds, but only as many columns as
# independent coordinates and speeds.
with warnings.catch_warnings():
warnings.filterwarnings("ignore", category=SymPyDeprecationWarning)
forcing_lin = KM.linearize()[0]
# As mentioned above, the size of the linearized forcing terms is expanded
# to include both q's and u's, so the mass matrix must have this done as
# well. This will likely be changed to be part of the linearized process,
# for future reference.
MM_full = KM.mass_matrix_full
MM_full_s = msubs(MM_full, val_dict)
forcing_lin_s = msubs(forcing_lin, KM.kindiffdict(), val_dict)
MM_full_s = MM_full_s.evalf()
forcing_lin_s = forcing_lin_s.evalf()
# Finally, we construct an "A" matrix for the form xdot = A x (x being the
# state vector, although in this case, the sizes are a little off). The
# following line extracts only the minimum entries required for eigenvalue
# analysis, which correspond to rows and columns for lean, steer, lean
# rate, and steer rate.
Amat = MM_full_s.inv() * forcing_lin_s
A = Amat.extract([1, 2, 4, 6], [1, 2, 3, 5])
# Precomputed for comparison
Res = Matrix([[ 0, 0, 1.0, 0],
[ 0, 0, 0, 1.0],
[9.48977444677355, -0.891197738059089*v**2 - 0.571523173729245, -0.105522449805691*v, -0.330515398992311*v],
[11.7194768719633, -1.97171508499972*v**2 + 30.9087533932407, 3.67680523332152*v, -3.08486552743311*v]])
# Actual eigenvalue comparison
eps = 1.e-12
for i in range(6):
error = Res.subs(v, i) - A.subs(v, i)
assert all(abs(x) < eps for x in error)
def define_controller(self, A, B, C, f, eta, phi):
"""
Define the Dynamic controller given by the differential equations:
.. math::
\\frac{dz(t)}{dt} = A.z(t) - B.f(\\sigma(t)) + \\eta\\left(w(t), \\frac{dw(t)}{dt}\\right)
.. math::
\\sigma(t) = C'.z
.. math::
u_0 = \\phi\\left(z(t), \\frac{dz(t)}{dt}\\right)
with z(t) the state vector, w(t) the input vector and t the time in seconds. the symbol ' refers to the transpose.
Conditions:
* A is Hurwitz,
* (A, B) should be controllable,
* (A, C) is observable,
* rank(B) = rang (C) = s <= n, with s the dimension of sigma, and n the number of states.
**HINT:** use create_variables() for an easy notation of the equations.
Parameters
-----------
A : array-like
Hurwitz matrix. Size: n x n
B : array-like
In combination with matrix A, the controllability is checked. The linear definition can be used. Size: s x n
C : array-like
In combination with matrix A, the observability is checked. The linear definition can be used. Size: n x 1
f : callable (lambda-function)
A (non)linear lambda function with argument sigma, which equals C'.z.
eta : array-like
The (non)linear relation between the inputs plus its derivatives to the change in state. Size: n x 1
phi : array-like
The (non)linear output equation. The equations should only contain states and its derivatives. Size: n x 1
Examples:
---------
See DyncamicController object.
"""
dim_states = self.states.shape[0]
# Allow scalar inputs
if np.isscalar(A):
A = [[A]]
if np.isscalar(B):
B = [[B]]
if np.isscalar(C):
C = [[C]]
if type(eta) not in (list, Matrix):
eta = [[eta]]
if type(phi) not in (list, Matrix):
phi = [[phi]]
if Matrix(A).shape[1] == dim_states:
if self.hurwitz(A):
self.A = Matrix(A)
else:
error_text = '[nlcontrol.systems.DynamicController] The A matrix should be Hurwitz.'
raise AssertionError(error_text)
else:
error_text = '[nlcontrol.systems.DynamicController] The number of columns of A should be equal to the number of states.'
raise AssertionError(error_text)
if Matrix(B).shape[0] == dim_states:
if self.controllability_linear(A, B):
self.B = Matrix(B)
else:
error_text = '[nlcontrol.systems.DynamicController] The system is not controllable.'
raise AssertionError(error_text)
else:
error_text = '[nlcontrol.systems.DynamicController] The number of rows of B should be equal to the number of states.'
raise AssertionError(error_text)
if Matrix(C).shape[0] == dim_states:
if self.observability_linear(A, C):
self.C = Matrix(C)
else:
error_text = '[nlcontrol.systems.DynamicController] The system is not observable.'
raise AssertionError(error_text)
else:
error_text = '[nlcontrol.systems.DynamicController] The number of rows of C should be equal to the number of states.'
raise AssertionError(error_text)
if type(f) is not Matrix:
if callable(f):
argument = self.C.T * Matrix(self.states)
#TODO: make an array of f
self.f = f(argument[0, 0])
elif f == 0:
self.f = 0
else:
error_text = '[nlcontrol.systems.DynamicController] Argument f should be a callable function or identical 0.'
raise AssertionError(error_text)
else:
self.f = f
def return_dynamic_symbols(expr):
try:
return find_dynamicsymbols(expr)
except:
return set()
if Matrix(eta).shape[0] == dim_states and Matrix(eta).shape[1] == 1:
# Check whether the expressions only contain inputs
if type(eta) is Matrix:
dynamic_symbols_eta = [
return_dynamic_symbols(eta_el[0])
for eta_el in eta.tolist()
]
else:
dynamic_symbols_eta = [
return_dynamic_symbols(eta_el)
for eta_el in list(itertools.chain(*eta))
]
dynamic_symbols_eta = set.union(*dynamic_symbols_eta)
if dynamic_symbols_eta <= (set(self.inputs)):
self.eta = Matrix(eta)
else:
error_text = '[nlcontrol.systems.DynamicController] Vector eta cannot contain other dynamic symbols than the inputs.'
raise AssertionError(error_text)
else:
error_text = '[nlcontrol.systems.DynamicController] Vector eta has an equal amount of columns as there are states. Eta has only one row.'
raise AssertionError(error_text)
# Check whether the expressions only contain inputs and derivatives of the input
if type(phi) is Matrix:
dynamic_symbols_phi = [
return_dynamic_symbols(phi_el[0]) for phi_el in phi.tolist()
]
else:
dynamic_symbols_phi = [
return_dynamic_symbols(phi_el)
for phi_el in list(itertools.chain(*phi))
]
dynamic_symbols_phi = set.union(*dynamic_symbols_phi)
if dynamic_symbols_phi <= (set(self.states) | set(self.dstates)):
self.phi = Matrix(phi)
else:
error_text = '[nlcontrol.systems.DynamicController] Vector phi cannot contain other dynamic symbols than the states and its derivatives.'
raise AssertionError(error_text)
state_equation = Array(self.A * Matrix(self.states) - self.B * self.f +
self.eta)
output_equation = Array(self.phi)
diff_states = []
for state in self.states:
diff_states.append(Derivative(state, Symbol('t')))
substitutions = dict(zip(diff_states, state_equation))
# print('Subs: ', substitutions)
output_equation = msubs(output_equation, substitutions)
self.system = DynamicalSystem(state_equation=state_equation,
output_equation=output_equation,
state=self.states,
input_=self.inputs)
constants = {
m: 10.0,
g: 9.81,
k: 100.0,
L_1_init: X_Y_L1_L2[i, 2],
L_2_init: X_Y_L1_L2[i, 3],
H: 20.0,
c: 10.0,
D: 3.0,
Izz: 3**2 * (1.0 / 3.0) * 10,
k_beta: 1.0,
c_beta: 1.0
}
M_op = me.msubs(M, op_point)
A_op = me.msubs(A, op_point)
# B_op = me.msubs(B, op_point)
perm_mat = linearizer.perm_mat
A_lin = perm_mat.T * M_op.LUsolve(A_op)
A_lin_constants = me.msubs(A_lin, constants)
A_sol = A_lin_constants.subs(op_point).doit()
A_np = np.array(np.array(A_sol), np.float)
eigenvals, eigenvects = np.linalg.eig(A_np)
eigen = eigenvals[0:5:2]
eigen_abs = np.abs(eigen)
damp = np.abs(np.real(eigen) / eigen_abs)
def test_bicycle():
if ON_TRAVIS:
skip("Too slow for travis.")
# Code to get equations of motion for a bicycle modeled as in:
# J.P Meijaard, Jim M Papadopoulos, Andy Ruina and A.L Schwab. Linearized
# dynamics equations for the balance and steer of a bicycle: a benchmark
# and review. Proceedings of The Royal Society (2007) 463, 1955-1982
# doi: 10.1098/rspa.2007.1857
# Note that this code has been crudely ported from Autolev, which is the
# reason for some of the unusual naming conventions. It was purposefully as
# similar as possible in order to aide debugging.
# Declare Coordinates & Speeds
# Simple definitions for qdots - qd = u
# Speeds are: yaw frame ang. rate, roll frame ang. rate, rear wheel frame
# ang. rate (spinning motion), frame ang. rate (pitching motion), steering
# frame ang. rate, and front wheel ang. rate (spinning motion).
# Wheel positions are ignorable coordinates, so they are not introduced.
q1, q2, q4, q5 = dynamicsymbols('q1 q2 q4 q5')
q1d, q2d, q4d, q5d = dynamicsymbols('q1 q2 q4 q5', 1)
u1, u2, u3, u4, u5, u6 = dynamicsymbols('u1 u2 u3 u4 u5 u6')
u1d, u2d, u3d, u4d, u5d, u6d = dynamicsymbols('u1 u2 u3 u4 u5 u6', 1)
# Declare System's Parameters
WFrad, WRrad, htangle, forkoffset = symbols(
'WFrad WRrad htangle forkoffset')
forklength, framelength, forkcg1 = symbols(
'forklength framelength forkcg1')
forkcg3, framecg1, framecg3, Iwr11 = symbols(
'forkcg3 framecg1 framecg3 Iwr11')
Iwr22, Iwf11, Iwf22, Iframe11 = symbols('Iwr22 Iwf11 Iwf22 Iframe11')
Iframe22, Iframe33, Iframe31, Ifork11 = symbols(
'Iframe22 Iframe33 Iframe31 Ifork11')
Ifork22, Ifork33, Ifork31, g = symbols('Ifork22 Ifork33 Ifork31 g')
mframe, mfork, mwf, mwr = symbols('mframe mfork mwf mwr')
# Set up reference frames for the system
# N - inertial
# Y - yaw
# R - roll
# WR - rear wheel, rotation angle is ignorable coordinate so not oriented
# Frame - bicycle frame
# TempFrame - statically rotated frame for easier reference inertia definition
# Fork - bicycle fork
# TempFork - statically rotated frame for easier reference inertia definition
# WF - front wheel, again posses a ignorable coordinate
N = ReferenceFrame('N')
Y = N.orientnew('Y', 'Axis', [q1, N.z])
R = Y.orientnew('R', 'Axis', [q2, Y.x])
Frame = R.orientnew('Frame', 'Axis', [q4 + htangle, R.y])
WR = ReferenceFrame('WR')
TempFrame = Frame.orientnew('TempFrame', 'Axis', [-htangle, Frame.y])
Fork = Frame.orientnew('Fork', 'Axis', [q5, Frame.x])
TempFork = Fork.orientnew('TempFork', 'Axis', [-htangle, Fork.y])
WF = ReferenceFrame('WF')
# Kinematics of the Bicycle First block of code is forming the positions of
# the relevant points
# rear wheel contact -> rear wheel mass center -> frame mass center +
# frame/fork connection -> fork mass center + front wheel mass center ->
# front wheel contact point
WR_cont = Point('WR_cont')
WR_mc = WR_cont.locatenew('WR_mc', WRrad * R.z)
Steer = WR_mc.locatenew('Steer', framelength * Frame.z)
Frame_mc = WR_mc.locatenew('Frame_mc',
-framecg1 * Frame.x + framecg3 * Frame.z)
Fork_mc = Steer.locatenew('Fork_mc', -forkcg1 * Fork.x + forkcg3 * Fork.z)
WF_mc = Steer.locatenew('WF_mc', forklength * Fork.x + forkoffset * Fork.z)
WF_cont = WF_mc.locatenew(
'WF_cont',
WFrad * (dot(Fork.y, Y.z) * Fork.y - Y.z).normalize())
# Set the angular velocity of each frame.
# Angular accelerations end up being calculated automatically by
# differentiating the angular velocities when first needed.
# u1 is yaw rate
# u2 is roll rate
# u3 is rear wheel rate
# u4 is frame pitch rate
# u5 is fork steer rate
# u6 is front wheel rate
Y.set_ang_vel(N, u1 * Y.z)
R.set_ang_vel(Y, u2 * R.x)
WR.set_ang_vel(Frame, u3 * Frame.y)
Frame.set_ang_vel(R, u4 * Frame.y)
Fork.set_ang_vel(Frame, u5 * Fork.x)
WF.set_ang_vel(Fork, u6 * Fork.y)
# Form the velocities of the previously defined points, using the 2 - point
# theorem (written out by hand here). Accelerations again are calculated
# automatically when first needed.
WR_cont.set_vel(N, 0)
WR_mc.v2pt_theory(WR_cont, N, WR)
Steer.v2pt_theory(WR_mc, N, Frame)
Frame_mc.v2pt_theory(WR_mc, N, Frame)
Fork_mc.v2pt_theory(Steer, N, Fork)
WF_mc.v2pt_theory(Steer, N, Fork)
WF_cont.v2pt_theory(WF_mc, N, WF)
# Sets the inertias of each body. Uses the inertia frame to construct the
# inertia dyadics. Wheel inertias are only defined by principle moments of
# inertia, and are in fact constant in the frame and fork reference frames;
# it is for this reason that the orientations of the wheels does not need
# to be defined. The frame and fork inertias are defined in the 'Temp'
# frames which are fixed to the appropriate body frames; this is to allow
# easier input of the reference values of the benchmark paper. Note that
# due to slightly different orientations, the products of inertia need to
# have their signs flipped; this is done later when entering the numerical
# value.
Frame_I = (inertia(TempFrame, Iframe11, Iframe22, Iframe33, 0, 0,
Iframe31), Frame_mc)
Fork_I = (inertia(TempFork, Ifork11, Ifork22, Ifork33, 0, 0,
Ifork31), Fork_mc)
WR_I = (inertia(Frame, Iwr11, Iwr22, Iwr11), WR_mc)
WF_I = (inertia(Fork, Iwf11, Iwf22, Iwf11), WF_mc)
# Declaration of the RigidBody containers. ::
BodyFrame = RigidBody('BodyFrame', Frame_mc, Frame, mframe, Frame_I)
BodyFork = RigidBody('BodyFork', Fork_mc, Fork, mfork, Fork_I)
BodyWR = RigidBody('BodyWR', WR_mc, WR, mwr, WR_I)
BodyWF = RigidBody('BodyWF', WF_mc, WF, mwf, WF_I)
# The kinematic differential equations; they are defined quite simply. Each
# entry in this list is equal to zero.
kd = [q1d - u1, q2d - u2, q4d - u4, q5d - u5]
# The nonholonomic constraints are the velocity of the front wheel contact
# point dotted into the X, Y, and Z directions; the yaw frame is used as it
# is "closer" to the front wheel (1 less DCM connecting them). These
# constraints force the velocity of the front wheel contact point to be 0
# in the inertial frame; the X and Y direction constraints enforce a
# "no-slip" condition, and the Z direction constraint forces the front
# wheel contact point to not move away from the ground frame, essentially
# replicating the holonomic constraint which does not allow the frame pitch
# to change in an invalid fashion.
conlist_speed = [
WF_cont.vel(N) & Y.x,
WF_cont.vel(N) & Y.y,
WF_cont.vel(N) & Y.z
]
# The holonomic constraint is that the position from the rear wheel contact
# point to the front wheel contact point when dotted into the
# normal-to-ground plane direction must be zero; effectively that the front
# and rear wheel contact points are always touching the ground plane. This
# is actually not part of the dynamic equations, but instead is necessary
# for the lineraization process.
conlist_coord = [WF_cont.pos_from(WR_cont) & Y.z]
# The force list; each body has the appropriate gravitational force applied
# at its mass center.
FL = [(Frame_mc, -mframe * g * Y.z), (Fork_mc, -mfork * g * Y.z),
(WF_mc, -mwf * g * Y.z), (WR_mc, -mwr * g * Y.z)]
BL = [BodyFrame, BodyFork, BodyWR, BodyWF]
# The N frame is the inertial frame, coordinates are supplied in the order
# of independent, dependent coordinates, as are the speeds. The kinematic
# differential equation are also entered here. Here the dependent speeds
# are specified, in the same order they were provided in earlier, along
# with the non-holonomic constraints. The dependent coordinate is also
# provided, with the holonomic constraint. Again, this is only provided
# for the linearization process.
KM = KanesMethod(N,
q_ind=[q1, q2, q5],
q_dependent=[q4],
configuration_constraints=conlist_coord,
u_ind=[u2, u3, u5],
u_dependent=[u1, u4, u6],
velocity_constraints=conlist_speed,
kd_eqs=kd)
with warnings.catch_warnings():
warnings.filterwarnings("ignore", category=SymPyDeprecationWarning)
(fr, frstar) = KM.kanes_equations(FL, BL)
# This is the start of entering in the numerical values from the benchmark
# paper to validate the eigen values of the linearized equations from this
# model to the reference eigen values. Look at the aforementioned paper for
# more information. Some of these are intermediate values, used to
# transform values from the paper into the coordinate systems used in this
# model.
PaperRadRear = 0.3
PaperRadFront = 0.35
HTA = evalf.N(pi / 2 - pi / 10)
TrailPaper = 0.08
rake = evalf.N(-(TrailPaper * sin(HTA) - (PaperRadFront * cos(HTA))))
PaperWb = 1.02
PaperFrameCgX = 0.3
PaperFrameCgZ = 0.9
PaperForkCgX = 0.9
PaperForkCgZ = 0.7
FrameLength = evalf.N(PaperWb * sin(HTA) -
(rake - (PaperRadFront - PaperRadRear) * cos(HTA)))
FrameCGNorm = evalf.N((PaperFrameCgZ - PaperRadRear -
(PaperFrameCgX / sin(HTA)) * cos(HTA)) * sin(HTA))
FrameCGPar = evalf.N(
(PaperFrameCgX / sin(HTA) +
(PaperFrameCgZ - PaperRadRear - PaperFrameCgX / sin(HTA) * cos(HTA)) *
cos(HTA)))
tempa = evalf.N((PaperForkCgZ - PaperRadFront))
tempb = evalf.N((PaperWb - PaperForkCgX))
tempc = evalf.N(sqrt(tempa**2 + tempb**2))
PaperForkL = evalf.N(
(PaperWb * cos(HTA) - (PaperRadFront - PaperRadRear) * sin(HTA)))
ForkCGNorm = evalf.N(rake +
(tempc * sin(pi / 2 - HTA - acos(tempa / tempc))))
ForkCGPar = evalf.N(tempc * cos((pi / 2 - HTA) - acos(tempa / tempc)) -
PaperForkL)
# Here is the final assembly of the numerical values. The symbol 'v' is the
# forward speed of the bicycle (a concept which only makes sense in the
# upright, static equilibrium case?). These are in a dictionary which will
# later be substituted in. Again the sign on the *product* of inertia
# values is flipped here, due to different orientations of coordinate
# systems.
v = symbols('v')
val_dict = {
WFrad: PaperRadFront,
WRrad: PaperRadRear,
htangle: HTA,
forkoffset: rake,
forklength: PaperForkL,
framelength: FrameLength,
forkcg1: ForkCGPar,
forkcg3: ForkCGNorm,
framecg1: FrameCGNorm,
framecg3: FrameCGPar,
Iwr11: 0.0603,
Iwr22: 0.12,
Iwf11: 0.1405,
Iwf22: 0.28,
Ifork11: 0.05892,
Ifork22: 0.06,
Ifork33: 0.00708,
Ifork31: 0.00756,
Iframe11: 9.2,
Iframe22: 11,
Iframe33: 2.8,
Iframe31: -2.4,
mfork: 4,
mframe: 85,
mwf: 3,
mwr: 2,
g: 9.81,
q1: 0,
q2: 0,
q4: 0,
q5: 0,
u1: 0,
u2: 0,
u3: v / PaperRadRear,
u4: 0,
u5: 0,
u6: v / PaperRadFront
}
# Linearizes the forcing vector; the equations are set up as MM udot =
# forcing, where MM is the mass matrix, udot is the vector representing the
# time derivatives of the generalized speeds, and forcing is a vector which
# contains both external forcing terms and internal forcing terms, such as
# centripital or coriolis forces. This actually returns a matrix with as
# many rows as *total* coordinates and speeds, but only as many columns as
# independent coordinates and speeds.
with warnings.catch_warnings():
warnings.filterwarnings("ignore", category=SymPyDeprecationWarning)
forcing_lin = KM.linearize()[0]
# As mentioned above, the size of the linearized forcing terms is expanded
# to include both q's and u's, so the mass matrix must have this done as
# well. This will likely be changed to be part of the linearized process,
# for future reference.
MM_full = KM.mass_matrix_full
MM_full_s = msubs(MM_full, val_dict)
forcing_lin_s = msubs(forcing_lin, KM.kindiffdict(), val_dict)
MM_full_s = MM_full_s.evalf()
forcing_lin_s = forcing_lin_s.evalf()
# Finally, we construct an "A" matrix for the form xdot = A x (x being the
# state vector, although in this case, the sizes are a little off). The
# following line extracts only the minimum entries required for eigenvalue
# analysis, which correspond to rows and columns for lean, steer, lean
# rate, and steer rate.
Amat = MM_full_s.inv() * forcing_lin_s
A = Amat.extract([1, 2, 4, 6], [1, 2, 3, 5])
# Precomputed for comparison
Res = Matrix([[0, 0, 1.0, 0], [0, 0, 0, 1.0],
[
9.48977444677355,
-0.891197738059089 * v**2 - 0.571523173729245,
-0.105522449805691 * v, -0.330515398992311 * v
],
[
11.7194768719633,
-1.97171508499972 * v**2 + 30.9087533932407,
3.67680523332152 * v, -3.08486552743311 * v
]])
# Actual eigenvalue comparison
eps = 1.e-12
for i in range(6):
error = Res.subs(v, i) - A.subs(v, i)
assert all(abs(x) < eps for x in error)
def msubs(self, *args):
return ExternalForce(me.msubs(self._Q, *args))
def msubs(self, *args):
return HomogenousTransform(me.msubs(self.E, *args))
def fixed_point_finder(p,
model,
vars_ls,
jac=True,
fixed_point_gen=None,
additional_func={}):
"""
Method to generate the flutter results for a model for each permutation of the parms in param_perms:
p - instance of Model Parameters
model - instance of symbolic model using params defined in p
param_perms - a list of tuples, each tuple consists of (Symbol, list of values)
calc_fixed_points - if True, will calc the fixed point for each permutation
"""
#get list of symbols to key in model
variables = [k for k, v in vars_ls]
model_mini = model.msubs(p.GetSubs(0, p.fp, ignore=variables))
# If caluclating fixed points generate require objective functions
if fixed_point_gen is None:
f = msubs((model_mini.f - model_mini.ExtForces.Q()),
{i: 0
for i in p.qd})
else:
f = fixed_point_gen(model_mini)
f_v0 = msubs(f, {sym.Symbol(p.V.name): 0})
func_obj = sym.lambdify((p.q, variables), f, "numpy")
func_obj_v0 = sym.lambdify((p.q, variables), f_v0, "numpy")
if jac:
func_jac_obj = sym.lambdify((p.q, variables), f.jacobian(p.q), "numpy")
# Get all possible combinations of the variables
perms = np.array(np.meshgrid(*[v for k, v in vars_ls])).T.reshape(
-1, len(vars_ls))
df_perms = [[(vars_ls[i][0], row[i]) for i in range(len(row))]
for row in perms]
string_perms = [{vars_ls[i][0].name: row[i]
for i in range(len(row))} for row in perms]
#Calc freqs and dampings
fixed_point_dataframe = []
qs = []
for i in range(len(df_perms)):
values = tuple([v for k, v in df_perms[i]])
# Calc fixed point
#set the initial guess (if v=0 set to FWT dropped doen else use previous result)
if string_perms[i]["V"] == 0:
guess = [0] * p.qs
guess[-1] = np.pi / 2
q = fsolve(lambda q, v: func_obj_v0(q, v)[:, 0],
guess,
factor=1,
args=(values, ))
else:
guess = [0] * p.qs #if len(qs)==0 else qs[-1]
guess[
-1] += 0.1 #if len(qs)==0 else 0 # gets you off the hill (where jacobian is zero for fwt...)
lb = [-np.inf] * p.qs
ub = [np.inf] * p.qs
lb[0] = -0.1
ub[0] = 0.1
lb[-1] = -2
ub[-1] = 2
bounds = (lb, ub)
q = root(lambda q, v: func_obj(q, values)[:, 0],
guess,
jac=func_jac_obj if jac else None,
args=(values, )).x
qs.append(q)
additional = {k: v(q, values) for k, v in additional_func.items()}
fixed_point_dataframe.append({**string_perms[i], **additional, 'q': q})
return pd.DataFrame(fixed_point_dataframe)
def GenV2RectWing(b_modes,
t_modes,
aero_model_class,
iwt=True,
iwb=True,
fwt_Iyy=True):
p = rw.base_params(b_modes + t_modes + 2)
#get shape functions for main wing
z_0, tau_0 = ma.ShapeFunctions_BN_TM(b_modes,
t_modes,
p.q[1:-1],
p.y_0,
p.x_0,
p.x_f0,
0,
factor=p.eta)
wing_bend = sym.atan(z_0.diff(p.y_0).subs({p.x_0: p.x_f0, p.y_0: p.s_0}))
#define wrefernce frames
wing_root_frame = ma.HomogenousTransform().Translate(0, 0,
p.q[0]).R_y(p.alpha_r)
wing_frame = wing_root_frame.Translate(p.x_0, p.y_0, z_0)
wing_flexural_frame = wing_frame.msubs({p.x_0: p.x_f0})
fwt_root_frame = wing_frame.msubs({
p.y_0: p.s_0,
p.x_0: p.x_f0
}).Translate(-p.x_f0, 0, 0).R_x(-p.q[-1])
fwt_flexural_frame = fwt_root_frame.Translate(p.x_f1, p.y_1, 0)
fwt_com_frame = fwt_root_frame.Translate(p.c / 2, p.s_1 / 2, 0)
#Create Element Mass Matriceis
M_wing = ele.MassMatrix(p.rho_t)
I_yy = 0
I_yy += sym.Rational(1, 2) * p.m_1 * p.c + p.m_1 * (p.c / 2)**2
M_fwt = ele.MassMatrix(p.m_1, I_xx=p.I_xx_1, I_yy=I_yy)
#Create Elements
inner_wing_ele = ele.FlexiElement(wing_root_frame,
M_wing,
p.x_0,
p.y_0,
z_0,
p.c,
p.s_0,
p.x_f0,
p.EI,
p.GJ,
gravityPot=True)
fwt_ele = ele.RigidElement(fwt_com_frame, M_fwt, True)
fwt_spring_ele = ele.Spring(p.q[-1] + wing_bend, p.k_fwt)
ac_ele = ele.RigidElement.PointMass(wing_root_frame,
p.m_ac,
gravityPotential=False)
ac_spring_ele = ele.Spring(p.q[0], p.k_ac)
# Create Inner Wing Aero Forces
wing_AeroForces = ef.AeroForce_1.PerUnitSpan(
p,
wing_flexural_frame,
p.a_0,
alphadot=tau_0 if isinstance(tau_0, int) else tau_0.diff(t),
M_thetadot=p.M_thetadot,
e=p.e_0,
rootAlpha=p.alpha_r,
deltaAlpha=tau_0,
alpha_zero=0,
w_g=p.w_g,
V=p.V * sym.cos(p.yaw)).integrate((p.y_0, 0, p.s_0))
#Create FWT Aero Forces
alpha_fwt = p.alpha_1
alphadot_fwt = p.alphadot_1
if not aero_model_class.rot:
fwt_AeroForces_perUnit = ef.AeroForce_1.PerUnitSpan(
p,
fwt_flexural_frame,
p.a_1,
alphadot=p.alphadot_1,
M_thetadot=p.M_thetadot,
e=p.e_1,
rootAlpha=p.alpha_1,
deltaAlpha=0,
alpha_zero=(p.y_1 / p.s_1) * p.tau_1,
#alpha_zero = 0,
w_g=sym.cos(p.alpha_1) * sym.cos(p.q[-1]) * p.w_g,
V=p.V * sym.cos(p.yaw))
else:
fwt_AeroForces_perUnit = ef.AeroForce_4.PerUnitSpan(
p,
fwt_flexural_frame,
p.a_1,
alphadot=p.alphadot_1,
M_thetadot=p.M_thetadot,
e=p.e_1,
rootAlpha=p.alpha_1,
alpha_zero=(p.y_1 / p.s_1) * p.tau_1,
#alpha_zero = 0,
stall_angle=0 if not aero_model_class.stall else p.alpha_s,
c_d_max=0 if not aero_model_class.drag else p.c_dmax,
w_g=sym.cos(p.alpha_1) * sym.cos(p.q[-1]) * p.w_g,
V=p.V * sym.cos(p.yaw))
#calcualte aero force on 5 discrete segments along the length of the fwt
forces = []
segments = 5
for i in range(segments):
seg_width = p.s_1 / segments
yi = seg_width / 2 + i * seg_width
forces.append(fwt_AeroForces_perUnit.subs({p.y_1: yi}) * seg_width)
Q = sym.Matrix([0] * p.qs)
for f in forces:
Q += f.Q()
fwt_AeroForces = ef.ExternalForce(Q)
# Setup AoA of FWT to sub into generated equations
if aero_model_class.aoa_model == AoAModel.GEM:
fwt_aoa = ma.GetAoA(p.alpha_r, p.yaw, p.Lambda, p.q[-1])
elif aero_model_class.aoa_model == AoAModel.SAM:
fwt_aoa = sym.atan(sym.sin(p.q[-1]) *
sym.sin(p.Lambda)) + p.alpha_r * sym.cos(p.q[-1])
elif aero_model_class.aoa_model == AoAModel.LIN:
fwt_aoa = (sym.atan(sym.tan(p.q[-1]) * sym.sin(p.Lambda)) +
p.alpha_r) * sym.cos(p.q[-1])
# augment fwt_aoa as required due to the shape of the inner wing
if iwb:
fwt_aoa = me.msubs(fwt_aoa, {p.q[-1]: p.q[-1] - wing_bend})
if iwt:
tau_s0 = tau_0.subs(p.y_0, p.s_0)
fwt_aoa = me.msubs(fwt_aoa, {p.alpha_r: p.alpha_r + tau_s0})
## Sub in Aero Forces
fwt_AeroForces = fwt_AeroForces.subs({
p.alpha_1: fwt_aoa,
p.alphadot_1: fwt_aoa.diff(t)
})
#Create Composite force
CompositeForce = ef.CompositeForce([wing_AeroForces, fwt_AeroForces])
# Create the SYmbolic Model
sm = ma.SymbolicModel.FromElementsAndForces(
p, [ac_ele, inner_wing_ele, fwt_ele, fwt_spring_ele, ac_spring_ele],
CompositeForce)
return sm, p
def _generate_closed_loop_eoms(self):
self.fr_plus_frstar_closed = me.msubs(self.fr_plus_frstar,
self.controller_dict)
