class Linearizer(object):
"""This object holds the general model form for a dynamic system.
This model is used for computing the linearized form of the system,
while properly dealing with constraints leading to dependent
coordinates and speeds.
Attributes
----------
f_0, f_1, f_2, f_3, f_4, f_c, f_v, f_a : Matrix
Matrices holding the general system form.
q, u, r : Matrix
Matrices holding the generalized coordinates, speeds, and
input vectors.
q_i, u_i : Matrix
Matrices of the independent generalized coordinates and speeds.
q_d, u_d : Matrix
Matrices of the dependent generalized coordinates and speeds.
perm_mat : Matrix
Permutation matrix such that [q_ind, u_ind]^T = perm_mat*[q, u]^T
"""
def __init__(self,
f_0,
f_1,
f_2,
f_3,
f_4,
f_c,
f_v,
f_a,
q,
u,
q_i=None,
q_d=None,
u_i=None,
u_d=None,
r=None,
lams=None):
"""
Parameters
----------
f_0, f_1, f_2, f_3, f_4, f_c, f_v, f_a : array_like
System of equations holding the general system form.
Supply empty array or Matrix if the parameter
doesn't exist.
q : array_like
The generalized coordinates.
u : array_like
The generalized speeds
q_i, u_i : array_like, optional
The independent generalized coordinates and speeds.
q_d, u_d : array_like, optional
The dependent generalized coordinates and speeds.
r : array_like, optional
The input variables.
lams : array_like, optional
The lagrange multipliers
"""
# Generalized equation form
self.f_0 = Matrix(f_0)
self.f_1 = Matrix(f_1)
self.f_2 = Matrix(f_2)
self.f_3 = Matrix(f_3)
self.f_4 = Matrix(f_4)
self.f_c = Matrix(f_c)
self.f_v = Matrix(f_v)
self.f_a = Matrix(f_a)
# Generalized equation variables
self.q = Matrix(q)
self.u = Matrix(u)
none_handler = lambda x: Matrix(x) if x else Matrix()
self.q_i = none_handler(q_i)
self.q_d = none_handler(q_d)
self.u_i = none_handler(u_i)
self.u_d = none_handler(u_d)
self.r = none_handler(r)
self.lams = none_handler(lams)
# Derivatives of generalized equation variables
self._qd = self.q.diff(dynamicsymbols._t)
self._ud = self.u.diff(dynamicsymbols._t)
# If the user doesn't actually use generalized variables, and the
# qd and u vectors have any intersecting variables, this can cause
# problems. We'll fix this with some hackery, and Dummy variables
dup_vars = set(self._qd).intersection(self.u)
self._qd_dup = Matrix(
[var if var not in dup_vars else Dummy() for var in self._qd])
# Derive dimesion terms
l = len(self.f_c)
m = len(self.f_v)
n = len(self.q)
o = len(self.u)
s = len(self.r)
k = len(self.lams)
dims = namedtuple('dims', ['l', 'm', 'n', 'o', 's', 'k'])
self._dims = dims(l, m, n, o, s, k)
self._setup_done = False
def _setup(self):
# Calculations here only need to be run once. They are moved out of
# the __init__ method to increase the speed of Linearizer creation.
self._form_permutation_matrices()
self._form_block_matrices()
self._form_coefficient_matrices()
self._setup_done = True
def _form_permutation_matrices(self):
"""Form the permutation matrices Pq and Pu."""
# Extract dimension variables
l, m, n, o, s, k = self._dims
# Compute permutation matrices
if n != 0:
self._Pq = permutation_matrix(self.q, Matrix([self.q_i, self.q_d]))
if l > 0:
self._Pqi = self._Pq[:, :-l]
self._Pqd = self._Pq[:, -l:]
else:
self._Pqi = self._Pq
self._Pqd = Matrix()
if o != 0:
self._Pu = permutation_matrix(self.u, Matrix([self.u_i, self.u_d]))
if m > 0:
self._Pui = self._Pu[:, :-m]
self._Pud = self._Pu[:, -m:]
else:
self._Pui = self._Pu
self._Pud = Matrix()
# Compute combination permutation matrix for computing A and B
P_col1 = Matrix([self._Pqi, zeros(o + k, n - l)])
P_col2 = Matrix([zeros(n, o - m), self._Pui, zeros(k, o - m)])
if P_col1:
if P_col2:
self.perm_mat = P_col1.row_join(P_col2)
else:
self.perm_mat = P_col1
else:
self.perm_mat = P_col2
def _form_coefficient_matrices(self):
"""Form the coefficient matrices C_0, C_1, and C_2."""
# Extract dimension variables
l, m, n, o, s, k = self._dims
# Build up the coefficient matrices C_0, C_1, and C_2
# If there are configuration constraints (l > 0), form C_0 as normal.
# If not, C_0 is I_(nxn). Note that this works even if n=0
if l > 0:
f_c_jac_q = self.f_c.jacobian(self.q)
self._C_0 = (
eye(n) - self._Pqd *
(f_c_jac_q * self._Pqd).LUsolve(f_c_jac_q)) * self._Pqi
else:
self._C_0 = eye(n)
# If there are motion constraints (m > 0), form C_1 and C_2 as normal.
# If not, C_1 is 0, and C_2 is I_(oxo). Note that this works even if
# o = 0.
if m > 0:
f_v_jac_u = self.f_v.jacobian(self.u)
temp = f_v_jac_u * self._Pud
if n != 0:
f_v_jac_q = self.f_v.jacobian(self.q)
self._C_1 = -self._Pud * temp.LUsolve(f_v_jac_q)
else:
self._C_1 = zeros(o, n)
self._C_2 = (eye(o) -
self._Pud * temp.LUsolve(f_v_jac_u)) * self._Pui
else:
self._C_1 = zeros(o, n)
self._C_2 = eye(o)
def _form_block_matrices(self):
"""Form the block matrices for composing M, A, and B."""
# Extract dimension variables
l, m, n, o, s, k = self._dims
# Block Matrix Definitions. These are only defined if under certain
# conditions. If undefined, an empty matrix is used instead
if n != 0:
self._M_qq = self.f_0.jacobian(self._qd)
self._A_qq = -(self.f_0 + self.f_1).jacobian(self.q)
else:
self._M_qq = Matrix()
self._A_qq = Matrix()
if n != 0 and m != 0:
self._M_uqc = self.f_a.jacobian(self._qd_dup)
self._A_uqc = -self.f_a.jacobian(self.q)
else:
self._M_uqc = Matrix()
self._A_uqc = Matrix()
if n != 0 and o - m + k != 0:
self._M_uqd = self.f_3.jacobian(self._qd_dup)
self._A_uqd = -(self.f_2 + self.f_3 + self.f_4).jacobian(self.q)
else:
self._M_uqd = Matrix()
self._A_uqd = Matrix()
if o != 0 and m != 0:
self._M_uuc = self.f_a.jacobian(self._ud)
self._A_uuc = -self.f_a.jacobian(self.u)
else:
self._M_uuc = Matrix()
self._A_uuc = Matrix()
if o != 0 and o - m + k != 0:
self._M_uud = self.f_2.jacobian(self._ud)
self._A_uud = -(self.f_2 + self.f_3).jacobian(self.u)
else:
self._M_uud = Matrix()
self._A_uud = Matrix()
if o != 0 and n != 0:
self._A_qu = -self.f_1.jacobian(self.u)
else:
self._A_qu = Matrix()
if k != 0 and o - m + k != 0:
self._M_uld = self.f_4.jacobian(self.lams)
else:
self._M_uld = Matrix()
if s != 0 and o - m + k != 0:
self._B_u = -self.f_3.jacobian(self.r)
else:
self._B_u = Matrix()
def linearize(self, op_point=None, A_and_B=False, simplify=False):
"""Linearize the system about the operating point. Note that
q_op, u_op, qd_op, ud_op must satisfy the equations of motion.
These may be either symbolic or numeric.
Parameters
----------
op_point : dict or iterable of dicts, optional
Dictionary or iterable of dictionaries containing the operating
point conditions. These will be substituted in to the linearized
system before the linearization is complete. Leave blank if you
want a completely symbolic form. Note that any reduction in
symbols (whether substituted for numbers or expressions with a
common parameter) will result in faster runtime.
A_and_B : bool, optional
If A_and_B=False (default), (M, A, B) is returned for forming
[M]*[q, u]^T = [A]*[q_ind, u_ind]^T + [B]r. If A_and_B=True,
(A, B) is returned for forming dx = [A]x + [B]r, where
x = [q_ind, u_ind]^T.
simplify : bool, optional
Determines if returned values are simplified before return.
For large expressions this may be time consuming. Default is False.
Potential Issues
----------------
Note that the process of solving with A_and_B=True is
computationally intensive if there are many symbolic parameters.
For this reason, it may be more desirable to use the default
A_and_B=False, returning M, A, and B. More values may then be
substituted in to these matrices later on. The state space form can
then be found as A = P.T*M.LUsolve(A), B = P.T*M.LUsolve(B), where
P = Linearizer.perm_mat.
"""
# Run the setup if needed:
if not self._setup_done:
self._setup()
# Compose dict of operating conditions
if isinstance(op_point, dict):
op_point_dict = op_point
elif isinstance(op_point, Iterable):
op_point_dict = {}
for op in op_point:
op_point_dict.update(op)
else:
op_point_dict = {}
# Extract dimension variables
l, m, n, o, s, k = self._dims
# Rename terms to shorten expressions
M_qq = self._M_qq
M_uqc = self._M_uqc
M_uqd = self._M_uqd
M_uuc = self._M_uuc
M_uud = self._M_uud
M_uld = self._M_uld
A_qq = self._A_qq
A_uqc = self._A_uqc
A_uqd = self._A_uqd
A_qu = self._A_qu
A_uuc = self._A_uuc
A_uud = self._A_uud
B_u = self._B_u
C_0 = self._C_0
C_1 = self._C_1
C_2 = self._C_2
# Build up Mass Matrix
# |M_qq 0_nxo 0_nxk|
# M = |M_uqc M_uuc 0_mxk|
# |M_uqd M_uud M_uld|
if o != 0:
col2 = Matrix([zeros(n, o), M_uuc, M_uud])
if k != 0:
col3 = Matrix([zeros(n + m, k), M_uld])
if n != 0:
col1 = Matrix([M_qq, M_uqc, M_uqd])
if o != 0 and k != 0:
M = col1.row_join(col2).row_join(col3)
elif o != 0:
M = col1.row_join(col2)
else:
M = col1
elif k != 0:
M = col2.row_join(col3)
else:
M = col2
M_eq = msubs(M, op_point_dict)
# Build up state coefficient matrix A
# |(A_qq + A_qu*C_1)*C_0 A_qu*C_2|
# A = |(A_uqc + A_uuc*C_1)*C_0 A_uuc*C_2|
# |(A_uqd + A_uud*C_1)*C_0 A_uud*C_2|
# Col 1 is only defined if n != 0
if n != 0:
r1c1 = A_qq
if o != 0:
r1c1 += (A_qu * C_1)
r1c1 = r1c1 * C_0
if m != 0:
r2c1 = A_uqc
if o != 0:
r2c1 += (A_uuc * C_1)
r2c1 = r2c1 * C_0
else:
r2c1 = Matrix()
if o - m + k != 0:
r3c1 = A_uqd
if o != 0:
r3c1 += (A_uud * C_1)
r3c1 = r3c1 * C_0
else:
r3c1 = Matrix()
col1 = Matrix([r1c1, r2c1, r3c1])
else:
col1 = Matrix()
# Col 2 is only defined if o != 0
if o != 0:
if n != 0:
r1c2 = A_qu * C_2
else:
r1c2 = Matrix()
if m != 0:
r2c2 = A_uuc * C_2
else:
r2c2 = Matrix()
if o - m + k != 0:
r3c2 = A_uud * C_2
else:
r3c2 = Matrix()
col2 = Matrix([r1c2, r2c2, r3c2])
else:
col2 = Matrix()
if col1:
if col2:
Amat = col1.row_join(col2)
else:
Amat = col1
else:
Amat = col2
Amat_eq = msubs(Amat, op_point_dict)
# Build up the B matrix if there are forcing variables
# |0_(n + m)xs|
# B = |B_u |
if s != 0 and o - m + k != 0:
Bmat = zeros(n + m, s).col_join(B_u)
Bmat_eq = msubs(Bmat, op_point_dict)
else:
Bmat_eq = Matrix()
# kwarg A_and_B indicates to return A, B for forming the equation
# dx = [A]x + [B]r, where x = [q_indnd, u_indnd]^T,
import sympy
if A_and_B:
A_cont = self.perm_mat.T * M_eq.LUsolve(Amat_eq)
if Bmat_eq:
B_cont = self.perm_mat.T * M_eq.LUsolve(Bmat_eq)
else:
# Bmat = Matrix([]), so no need to sub
B_cont = Bmat_eq
if simplify:
sympy.simplify(A_cont)
sympy.simplify(B_cont)
return A_cont, B_cont
# Otherwise return M, A, B for forming the equation
# [M]dx = [A]x + [B]r, where x = [q, u]^T
else:
if simplify:
sympy.simplify(M_eq)
sympy.simplify(Amat_eq)
sympy.simplify(Bmat_eq)
return M_eq, Amat_eq, Bmat_eq
class LagrangesMethod(object):
"""Lagrange's method object.
This object generates the equations of motion in a two step procedure. The
first step involves the initialization of LagrangesMethod by supplying the
Lagrangian and the generalized coordinates, at the bare minimum. If there
are any constraint equations, they can be supplied as keyword arguments.
The Lagrange multipliers are automatically generated and are equal in
number to the constraint equations. Similarly any non-conservative forces
can be supplied in an iterable (as described below and also shown in the
example) along with a ReferenceFrame. This is also discussed further in the
__init__ method.
Attributes
==========
q, u : Matrix
Matrices of the generalized coordinates and speeds
forcelist : iterable
Iterable of (Point, vector) or (ReferenceFrame, vector) tuples
describing the forces on the system.
bodies : iterable
Iterable containing the rigid bodies and particles of the system.
mass_matrix : Matrix
The system's mass matrix
forcing : Matrix
The system's forcing vector
mass_matrix_full : Matrix
The "mass matrix" for the qdot's, qdoubledot's, and the
lagrange multipliers (lam)
forcing_full : Matrix
The forcing vector for the qdot's, qdoubledot's and
lagrange multipliers (lam)
Examples
========
This is a simple example for a one degree of freedom translational
spring-mass-damper.
In this example, we first need to do the kinematics.
This involves creating generalized coordinates and their derivatives.
Then we create a point and set its velocity in a frame.
>>> from sympy.physics.mechanics import LagrangesMethod, Lagrangian
>>> from sympy.physics.mechanics import ReferenceFrame, Particle, Point
>>> from sympy.physics.mechanics import dynamicsymbols
>>> from sympy import symbols
>>> q = dynamicsymbols('q')
>>> qd = dynamicsymbols('q', 1)
>>> m, k, b = symbols('m k b')
>>> N = ReferenceFrame('N')
>>> P = Point('P')
>>> P.set_vel(N, qd * N.x)
We need to then prepare the information as required by LagrangesMethod to
generate equations of motion.
First we create the Particle, which has a point attached to it.
Following this the lagrangian is created from the kinetic and potential
energies.
Then, an iterable of nonconservative forces/torques must be constructed,
where each item is a (Point, Vector) or (ReferenceFrame, Vector) tuple,
with the Vectors representing the nonconservative forces or torques.
>>> Pa = Particle('Pa', P, m)
>>> Pa.potential_energy = k * q**2 / 2.0
>>> L = Lagrangian(N, Pa)
>>> fl = [(P, -b * qd * N.x)]
Finally we can generate the equations of motion.
First we create the LagrangesMethod object. To do this one must supply
the Lagrangian, and the generalized coordinates. The constraint equations,
the forcelist, and the inertial frame may also be provided, if relevant.
Next we generate Lagrange's equations of motion, such that:
Lagrange's equations of motion = 0.
We have the equations of motion at this point.
>>> l = LagrangesMethod(L, [q], forcelist = fl, frame = N)
>>> print(l.form_lagranges_equations())
Matrix([[b*Derivative(q(t), t) + 1.0*k*q(t) + m*Derivative(q(t), (t, 2))]])
We can also solve for the states using the 'rhs' method.
>>> print(l.rhs())
Matrix([[Derivative(q(t), t)], [(-b*Derivative(q(t), t) - 1.0*k*q(t))/m]])
Please refer to the docstrings on each method for more details.
"""
def __init__(self,
Lagrangian,
qs,
forcelist=None,
bodies=None,
frame=None,
hol_coneqs=None,
nonhol_coneqs=None):
"""Supply the following for the initialization of LagrangesMethod
Lagrangian : Sympifyable
qs : array_like
The generalized coordinates
hol_coneqs : array_like, optional
The holonomic constraint equations
nonhol_coneqs : array_like, optional
The nonholonomic constraint equations
forcelist : iterable, optional
Takes an iterable of (Point, Vector) or (ReferenceFrame, Vector)
tuples which represent the force at a point or torque on a frame.
This feature is primarily to account for the nonconservative forces
and/or moments.
bodies : iterable, optional
Takes an iterable containing the rigid bodies and particles of the
system.
frame : ReferenceFrame, optional
Supply the inertial frame. This is used to determine the
generalized forces due to non-conservative forces.
"""
self._L = Matrix([sympify(Lagrangian)])
self.eom = None
self._m_cd = Matrix() # Mass Matrix of differentiated coneqs
self._m_d = Matrix() # Mass Matrix of dynamic equations
self._f_cd = Matrix() # Forcing part of the diff coneqs
self._f_d = Matrix() # Forcing part of the dynamic equations
self.lam_coeffs = Matrix() # The coeffecients of the multipliers
forcelist = forcelist if forcelist else []
if not iterable(forcelist):
raise TypeError('Force pairs must be supplied in an iterable.')
self._forcelist = forcelist
if frame and not isinstance(frame, ReferenceFrame):
raise TypeError('frame must be a valid ReferenceFrame')
self._bodies = bodies
self.inertial = frame
self.lam_vec = Matrix()
self._term1 = Matrix()
self._term2 = Matrix()
self._term3 = Matrix()
self._term4 = Matrix()
# Creating the qs, qdots and qdoubledots
if not iterable(qs):
raise TypeError('Generalized coordinates must be an iterable')
self._q = Matrix(qs)
self._qdots = self.q.diff(dynamicsymbols._t)
self._qdoubledots = self._qdots.diff(dynamicsymbols._t)
mat_build = lambda x: Matrix(x) if x else Matrix()
hol_coneqs = mat_build(hol_coneqs)
nonhol_coneqs = mat_build(nonhol_coneqs)
self.coneqs = Matrix(
[hol_coneqs.diff(dynamicsymbols._t), nonhol_coneqs])
self._hol_coneqs = hol_coneqs
def form_lagranges_equations(self):
"""Method to form Lagrange's equations of motion.
Returns a vector of equations of motion using Lagrange's equations of
the second kind.
"""
qds = self._qdots
qdd_zero = dict((i, 0) for i in self._qdoubledots)
n = len(self.q)
# Internally we represent the EOM as four terms:
# EOM = term1 - term2 - term3 - term4 = 0
# First term
self._term1 = self._L.jacobian(qds)
self._term1 = self._term1.diff(dynamicsymbols._t).T
# Second term
self._term2 = self._L.jacobian(self.q).T
# Third term
if self.coneqs:
coneqs = self.coneqs
m = len(coneqs)
# Creating the multipliers
self.lam_vec = Matrix(dynamicsymbols('lam1:' + str(m + 1)))
self.lam_coeffs = -coneqs.jacobian(qds)
self._term3 = self.lam_coeffs.T * self.lam_vec
# Extracting the coeffecients of the qdds from the diff coneqs
diffconeqs = coneqs.diff(dynamicsymbols._t)
self._m_cd = diffconeqs.jacobian(self._qdoubledots)
# The remaining terms i.e. the 'forcing' terms in diff coneqs
self._f_cd = -diffconeqs.subs(qdd_zero)
else:
self._term3 = zeros(n, 1)
# Fourth term
if self.forcelist:
N = self.inertial
self._term4 = zeros(n, 1)
for i, qd in enumerate(qds):
flist = zip(*_f_list_parser(self.forcelist, N))
self._term4[i] = sum(v.diff(qd, N) & f for (v, f) in flist)
else:
self._term4 = zeros(n, 1)
# Form the dynamic mass and forcing matrices
without_lam = self._term1 - self._term2 - self._term4
self._m_d = without_lam.jacobian(self._qdoubledots)
self._f_d = -without_lam.subs(qdd_zero)
# Form the EOM
self.eom = without_lam - self._term3
return self.eom
@property
def mass_matrix(self):
"""Returns the mass matrix, which is augmented by the Lagrange
multipliers, if necessary.
If the system is described by 'n' generalized coordinates and there are
no constraint equations then an n X n matrix is returned.
If there are 'n' generalized coordinates and 'm' constraint equations
have been supplied during initialization then an n X (n+m) matrix is
returned. The (n + m - 1)th and (n + m)th columns contain the
coefficients of the Lagrange multipliers.
"""
if self.eom is None:
raise ValueError('Need to compute the equations of motion first')
if self.coneqs:
return (self._m_d).row_join(self.lam_coeffs.T)
else:
return self._m_d
@property
def mass_matrix_full(self):
"""Augments the coefficients of qdots to the mass_matrix."""
if self.eom is None:
raise ValueError('Need to compute the equations of motion first')
n = len(self.q)
m = len(self.coneqs)
row1 = eye(n).row_join(zeros(n, n + m))
row2 = zeros(n, n).row_join(self.mass_matrix)
if self.coneqs:
row3 = zeros(m, n).row_join(self._m_cd).row_join(zeros(m, m))
return row1.col_join(row2).col_join(row3)
else:
return row1.col_join(row2)
@property
def forcing(self):
"""Returns the forcing vector from 'lagranges_equations' method."""
if self.eom is None:
raise ValueError('Need to compute the equations of motion first')
return self._f_d
@property
def forcing_full(self):
"""Augments qdots to the forcing vector above."""
if self.eom is None:
raise ValueError('Need to compute the equations of motion first')
if self.coneqs:
return self._qdots.col_join(self.forcing).col_join(self._f_cd)
else:
return self._qdots.col_join(self.forcing)
def to_linearizer(self, q_ind=None, qd_ind=None, q_dep=None, qd_dep=None):
"""Returns an instance of the Linearizer class, initiated from the
data in the LagrangesMethod class. This may be more desirable than using
the linearize class method, as the Linearizer object will allow more
efficient recalculation (i.e. about varying operating points).
Parameters
==========
q_ind, qd_ind : array_like, optional
The independent generalized coordinates and speeds.
q_dep, qd_dep : array_like, optional
The dependent generalized coordinates and speeds.
"""
# Compose vectors
t = dynamicsymbols._t
q = self.q
u = self._qdots
ud = u.diff(t)
# Get vector of lagrange multipliers
lams = self.lam_vec
mat_build = lambda x: Matrix(x) if x else Matrix()
q_i = mat_build(q_ind)
q_d = mat_build(q_dep)
u_i = mat_build(qd_ind)
u_d = mat_build(qd_dep)
# Compose general form equations
f_c = self._hol_coneqs
f_v = self.coneqs
f_a = f_v.diff(t)
f_0 = u
f_1 = -u
f_2 = self._term1
f_3 = -(self._term2 + self._term4)
f_4 = -self._term3
# Check that there are an appropriate number of independent and
# dependent coordinates
if len(q_d) != len(f_c) or len(u_d) != len(f_v):
raise ValueError(
("Must supply {:} dependent coordinates, and " +
"{:} dependent speeds").format(len(f_c), len(f_v)))
if set(Matrix([q_i, q_d])) != set(q):
raise ValueError("Must partition q into q_ind and q_dep, with " +
"no extra or missing symbols.")
if set(Matrix([u_i, u_d])) != set(u):
raise ValueError("Must partition qd into qd_ind and qd_dep, " +
"with no extra or missing symbols.")
# Find all other dynamic symbols, forming the forcing vector r.
# Sort r to make it canonical.
insyms = set(Matrix([q, u, ud, lams]))
r = list(find_dynamicsymbols(f_3, insyms))
r.sort(key=default_sort_key)
# Check for any derivatives of variables in r that are also found in r.
for i in r:
if diff(i, dynamicsymbols._t) in r:
raise ValueError('Cannot have derivatives of specified \
quantities when linearizing forcing terms.')
return Linearizer(f_0, f_1, f_2, f_3, f_4, f_c, f_v, f_a, q, u, q_i,
q_d, u_i, u_d, r, lams)
def linearize(self,
q_ind=None,
qd_ind=None,
q_dep=None,
qd_dep=None,
**kwargs):
"""Linearize the equations of motion about a symbolic operating point.
If kwarg A_and_B is False (default), returns M, A, B, r for the
linearized form, M*[q', u']^T = A*[q_ind, u_ind]^T + B*r.
If kwarg A_and_B is True, returns A, B, r for the linearized form
dx = A*x + B*r, where x = [q_ind, u_ind]^T. Note that this is
computationally intensive if there are many symbolic parameters. For
this reason, it may be more desirable to use the default A_and_B=False,
returning M, A, and B. Values may then be substituted in to these
matrices, and the state space form found as
A = P.T*M.inv()*A, B = P.T*M.inv()*B, where P = Linearizer.perm_mat.
In both cases, r is found as all dynamicsymbols in the equations of
motion that are not part of q, u, q', or u'. They are sorted in
canonical form.
The operating points may be also entered using the ``op_point`` kwarg.
This takes a dictionary of {symbol: value}, or a an iterable of such
dictionaries. The values may be numeric or symbolic. The more values
you can specify beforehand, the faster this computation will run.
For more documentation, please see the ``Linearizer`` class."""
linearizer = self.to_linearizer(q_ind, qd_ind, q_dep, qd_dep)
result = linearizer.linearize(**kwargs)
return result + (linearizer.r, )
def solve_multipliers(self, op_point=None, sol_type='dict'):
"""Solves for the values of the lagrange multipliers symbolically at
the specified operating point
Parameters
==========
op_point : dict or iterable of dicts, optional
Point at which to solve at. The operating point is specified as
a dictionary or iterable of dictionaries of {symbol: value}. The
value may be numeric or symbolic itself.
sol_type : str, optional
Solution return type. Valid options are:
- 'dict': A dict of {symbol : value} (default)
- 'Matrix': An ordered column matrix of the solution
"""
# Determine number of multipliers
k = len(self.lam_vec)
if k == 0:
raise ValueError(
"System has no lagrange multipliers to solve for.")
# Compose dict of operating conditions
if isinstance(op_point, dict):
op_point_dict = op_point
elif iterable(op_point):
op_point_dict = {}
for op in op_point:
op_point_dict.update(op)
elif op_point is None:
op_point_dict = {}
else:
raise TypeError("op_point must be either a dictionary or an "
"iterable of dictionaries.")
# Compose the system to be solved
mass_matrix = self.mass_matrix.col_join(
(-self.lam_coeffs.row_join(zeros(k, k))))
force_matrix = self.forcing.col_join(self._f_cd)
# Sub in the operating point
mass_matrix = msubs(mass_matrix, op_point_dict)
force_matrix = msubs(force_matrix, op_point_dict)
# Solve for the multipliers
sol_list = mass_matrix.LUsolve(-force_matrix)[-k:]
if sol_type == 'dict':
return dict(zip(self.lam_vec, sol_list))
elif sol_type == 'Matrix':
return Matrix(sol_list)
else:
raise ValueError("Unknown sol_type {:}.".format(sol_type))
def rhs(self, inv_method=None, **kwargs):
"""Returns equations that can be solved numerically
Parameters
==========
inv_method : str
The specific sympy inverse matrix calculation method to use. For a
list of valid methods, see
:meth:`~sympy.matrices.matrices.MatrixBase.inv`
"""
if inv_method is None:
self._rhs = self.mass_matrix_full.LUsolve(self.forcing_full)
else:
self._rhs = (
self.mass_matrix_full.inv(inv_method, try_block_diag=True) *
self.forcing_full)
return self._rhs
@property
def q(self):
return self._q
@property
def u(self):
return self._qdots
@property
def bodies(self):
return self._bodies
@property
def forcelist(self):
return self._forcelist
class Linearizer(object):
"""This object holds the general model form for a dynamic system.
This model is used for computing the linearized form of the system,
while properly dealing with constraints leading to dependent
coordinates and speeds.
Attributes
----------
f_0, f_1, f_2, f_3, f_4, f_c, f_v, f_a : Matrix
Matrices holding the general system form.
q, u, r : Matrix
Matrices holding the generalized coordinates, speeds, and
input vectors.
q_i, u_i : Matrix
Matrices of the independent generalized coordinates and speeds.
q_d, u_d : Matrix
Matrices of the dependent generalized coordinates and speeds.
perm_mat : Matrix
Permutation matrix such that [q_ind, u_ind]^T = perm_mat*[q, u]^T
"""
def __init__(self, f_0, f_1, f_2, f_3, f_4, f_c, f_v, f_a, q, u,
q_i=None, q_d=None, u_i=None, u_d=None, r=None, lams=None):
"""
Parameters
----------
f_0, f_1, f_2, f_3, f_4, f_c, f_v, f_a : array_like
System of equations holding the general system form.
Supply empty array or Matrix if the parameter
doesn't exist.
q : array_like
The generalized coordinates.
u : array_like
The generalized speeds
q_i, u_i : array_like, optional
The independent generalized coordinates and speeds.
q_d, u_d : array_like, optional
The dependent generalized coordinates and speeds.
r : array_like, optional
The input variables.
lams : array_like, optional
The lagrange multipliers
"""
# Generalized equation form
self.f_0 = Matrix(f_0)
self.f_1 = Matrix(f_1)
self.f_2 = Matrix(f_2)
self.f_3 = Matrix(f_3)
self.f_4 = Matrix(f_4)
self.f_c = Matrix(f_c)
self.f_v = Matrix(f_v)
self.f_a = Matrix(f_a)
# Generalized equation variables
self.q = Matrix(q)
self.u = Matrix(u)
none_handler = lambda x: Matrix(x) if x else Matrix()
self.q_i = none_handler(q_i)
self.q_d = none_handler(q_d)
self.u_i = none_handler(u_i)
self.u_d = none_handler(u_d)
self.r = none_handler(r)
self.lams = none_handler(lams)
# Derivatives of generalized equation variables
self._qd = self.q.diff(dynamicsymbols._t)
self._ud = self.u.diff(dynamicsymbols._t)
# If the user doesn't actually use generalized variables, and the
# qd and u vectors have any intersecting variables, this can cause
# problems. We'll fix this with some hackery, and Dummy variables
dup_vars = set(self._qd).intersection(self.u)
self._qd_dup = Matrix([var if var not in dup_vars else Dummy()
for var in self._qd])
# Derive dimesion terms
l = len(self.f_c)
m = len(self.f_v)
n = len(self.q)
o = len(self.u)
s = len(self.r)
k = len(self.lams)
dims = namedtuple('dims', ['l', 'm', 'n', 'o', 's', 'k'])
self._dims = dims(l, m, n, o, s, k)
self._setup_done = False
def _setup(self):
# Calculations here only need to be run once. They are moved out of
# the __init__ method to increase the speed of Linearizer creation.
self._form_permutation_matrices()
self._form_block_matrices()
self._form_coefficient_matrices()
self._setup_done = True
def _form_permutation_matrices(self):
"""Form the permutation matrices Pq and Pu."""
# Extract dimension variables
l, m, n, o, s, k = self._dims
# Compute permutation matrices
if n != 0:
self._Pq = permutation_matrix(self.q, Matrix([self.q_i, self.q_d]))
if l > 0:
self._Pqi = self._Pq[:, :-l]
self._Pqd = self._Pq[:, -l:]
else:
self._Pqi = self._Pq
self._Pqd = Matrix()
if o != 0:
self._Pu = permutation_matrix(self.u, Matrix([self.u_i, self.u_d]))
if m > 0:
self._Pui = self._Pu[:, :-m]
self._Pud = self._Pu[:, -m:]
else:
self._Pui = self._Pu
self._Pud = Matrix()
# Compute combination permutation matrix for computing A and B
P_col1 = Matrix([self._Pqi, zeros(o + k, n - l)])
P_col2 = Matrix([zeros(n, o - m), self._Pui, zeros(k, o - m)])
if P_col1:
if P_col2:
self.perm_mat = P_col1.row_join(P_col2)
else:
self.perm_mat = P_col1
else:
self.perm_mat = P_col2
def _form_coefficient_matrices(self):
"""Form the coefficient matrices C_0, C_1, and C_2."""
# Extract dimension variables
l, m, n, o, s, k = self._dims
# Build up the coefficient matrices C_0, C_1, and C_2
# If there are configuration constraints (l > 0), form C_0 as normal.
# If not, C_0 is I_(nxn). Note that this works even if n=0
if l > 0:
f_c_jac_q = self.f_c.jacobian(self.q)
self._C_0 = (eye(n) - self._Pqd * (f_c_jac_q *
self._Pqd).LUsolve(f_c_jac_q)) * self._Pqi
else:
self._C_0 = eye(n)
# If there are motion constraints (m > 0), form C_1 and C_2 as normal.
# If not, C_1 is 0, and C_2 is I_(oxo). Note that this works even if
# o = 0.
if m > 0:
f_v_jac_u = self.f_v.jacobian(self.u)
temp = f_v_jac_u * self._Pud
if n != 0:
f_v_jac_q = self.f_v.jacobian(self.q)
self._C_1 = -self._Pud * temp.LUsolve(f_v_jac_q)
else:
self._C_1 = zeros(o, n)
self._C_2 = (eye(o) - self._Pud *
temp.LUsolve(f_v_jac_u)) * self._Pui
else:
self._C_1 = zeros(o, n)
self._C_2 = eye(o)
def _form_block_matrices(self):
"""Form the block matrices for composing M, A, and B."""
# Extract dimension variables
l, m, n, o, s, k = self._dims
# Block Matrix Definitions. These are only defined if under certain
# conditions. If undefined, an empty matrix is used instead
if n != 0:
self._M_qq = self.f_0.jacobian(self._qd)
self._A_qq = -(self.f_0 + self.f_1).jacobian(self.q)
else:
self._M_qq = Matrix()
self._A_qq = Matrix()
if n != 0 and m != 0:
self._M_uqc = self.f_a.jacobian(self._qd_dup)
self._A_uqc = -self.f_a.jacobian(self.q)
else:
self._M_uqc = Matrix()
self._A_uqc = Matrix()
if n != 0 and o - m + k != 0:
self._M_uqd = self.f_3.jacobian(self._qd_dup)
self._A_uqd = -(self.f_2 + self.f_3 + self.f_4).jacobian(self.q)
else:
self._M_uqd = Matrix()
self._A_uqd = Matrix()
if o != 0 and m != 0:
self._M_uuc = self.f_a.jacobian(self._ud)
self._A_uuc = -self.f_a.jacobian(self.u)
else:
self._M_uuc = Matrix()
self._A_uuc = Matrix()
if o != 0 and o - m + k != 0:
self._M_uud = self.f_2.jacobian(self._ud)
self._A_uud = -(self.f_2 + self.f_3).jacobian(self.u)
else:
self._M_uud = Matrix()
self._A_uud = Matrix()
if o != 0 and n != 0:
self._A_qu = -self.f_1.jacobian(self.u)
else:
self._A_qu = Matrix()
if k != 0 and o - m + k != 0:
self._M_uld = self.f_4.jacobian(self.lams)
else:
self._M_uld = Matrix()
if s != 0 and o - m + k != 0:
self._B_u = -self.f_3.jacobian(self.r)
else:
self._B_u = Matrix()
def linearize(self, op_point=None, A_and_B=False, simplify=False):
"""Linearize the system about the operating point. Note that
q_op, u_op, qd_op, ud_op must satisfy the equations of motion.
These may be either symbolic or numeric.
Parameters
----------
op_point : dict or iterable of dicts, optional
Dictionary or iterable of dictionaries containing the operating
point conditions. These will be substituted in to the linearized
system before the linearization is complete. Leave blank if you
want a completely symbolic form. Note that any reduction in
symbols (whether substituted for numbers or expressions with a
common parameter) will result in faster runtime.
A_and_B : bool, optional
If A_and_B=False (default), (M, A, B) is returned for forming
[M]*[q, u]^T = [A]*[q_ind, u_ind]^T + [B]r. If A_and_B=True,
(A, B) is returned for forming dx = [A]x + [B]r, where
x = [q_ind, u_ind]^T.
simplify : bool, optional
Determines if returned values are simplified before return.
For large expressions this may be time consuming. Default is False.
Potential Issues
----------------
Note that the process of solving with A_and_B=True is
computationally intensive if there are many symbolic parameters.
For this reason, it may be more desirable to use the default
A_and_B=False, returning M, A, and B. More values may then be
substituted in to these matrices later on. The state space form can
then be found as A = P.T*M.LUsolve(A), B = P.T*M.LUsolve(B), where
P = Linearizer.perm_mat.
"""
# Run the setup if needed:
if not self._setup_done:
self._setup()
# Compose dict of operating conditions
if isinstance(op_point, dict):
op_point_dict = op_point
elif isinstance(op_point, Iterable):
op_point_dict = {}
for op in op_point:
op_point_dict.update(op)
else:
op_point_dict = {}
# Extract dimension variables
l, m, n, o, s, k = self._dims
# Rename terms to shorten expressions
M_qq = self._M_qq
M_uqc = self._M_uqc
M_uqd = self._M_uqd
M_uuc = self._M_uuc
M_uud = self._M_uud
M_uld = self._M_uld
A_qq = self._A_qq
A_uqc = self._A_uqc
A_uqd = self._A_uqd
A_qu = self._A_qu
A_uuc = self._A_uuc
A_uud = self._A_uud
B_u = self._B_u
C_0 = self._C_0
C_1 = self._C_1
C_2 = self._C_2
# Build up Mass Matrix
# |M_qq 0_nxo 0_nxk|
# M = |M_uqc M_uuc 0_mxk|
# |M_uqd M_uud M_uld|
if o != 0:
col2 = Matrix([zeros(n, o), M_uuc, M_uud])
if k != 0:
col3 = Matrix([zeros(n + m, k), M_uld])
if n != 0:
col1 = Matrix([M_qq, M_uqc, M_uqd])
if o != 0 and k != 0:
M = col1.row_join(col2).row_join(col3)
elif o != 0:
M = col1.row_join(col2)
else:
M = col1
elif k != 0:
M = col2.row_join(col3)
else:
M = col2
M_eq = msubs(M, op_point_dict)
# Build up state coefficient matrix A
# |(A_qq + A_qu*C_1)*C_0 A_qu*C_2|
# A = |(A_uqc + A_uuc*C_1)*C_0 A_uuc*C_2|
# |(A_uqd + A_uud*C_1)*C_0 A_uud*C_2|
# Col 1 is only defined if n != 0
if n != 0:
r1c1 = A_qq
if o != 0:
r1c1 += (A_qu * C_1)
r1c1 = r1c1 * C_0
if m != 0:
r2c1 = A_uqc
if o != 0:
r2c1 += (A_uuc * C_1)
r2c1 = r2c1 * C_0
else:
r2c1 = Matrix()
if o - m + k != 0:
r3c1 = A_uqd
if o != 0:
r3c1 += (A_uud * C_1)
r3c1 = r3c1 * C_0
else:
r3c1 = Matrix()
col1 = Matrix([r1c1, r2c1, r3c1])
else:
col1 = Matrix()
# Col 2 is only defined if o != 0
if o != 0:
if n != 0:
r1c2 = A_qu * C_2
else:
r1c2 = Matrix()
if m != 0:
r2c2 = A_uuc * C_2
else:
r2c2 = Matrix()
if o - m + k != 0:
r3c2 = A_uud * C_2
else:
r3c2 = Matrix()
col2 = Matrix([r1c2, r2c2, r3c2])
else:
col2 = Matrix()
if col1:
if col2:
Amat = col1.row_join(col2)
else:
Amat = col1
else:
Amat = col2
Amat_eq = msubs(Amat, op_point_dict)
# Build up the B matrix if there are forcing variables
# |0_(n + m)xs|
# B = |B_u |
if s != 0 and o - m + k != 0:
Bmat = zeros(n + m, s).col_join(B_u)
Bmat_eq = msubs(Bmat, op_point_dict)
else:
Bmat_eq = Matrix()
# kwarg A_and_B indicates to return A, B for forming the equation
# dx = [A]x + [B]r, where x = [q_indnd, u_indnd]^T,
if A_and_B:
A_cont = self.perm_mat.T * M_eq.LUsolve(Amat_eq)
if Bmat_eq:
B_cont = self.perm_mat.T * M_eq.LUsolve(Bmat_eq)
else:
# Bmat = Matrix([]), so no need to sub
B_cont = Bmat_eq
if simplify:
A_cont.simplify()
B_cont.simplify()
return A_cont, B_cont
# Otherwise return M, A, B for forming the equation
# [M]dx = [A]x + [B]r, where x = [q, u]^T
else:
if simplify:
M_eq.simplify()
Amat_eq.simplify()
Bmat_eq.simplify()
return M_eq, Amat_eq, Bmat_eq
class LagrangesMethod(object):
"""Lagrange's method object.
This object generates the equations of motion in a two step procedure. The
first step involves the initialization of LagrangesMethod by supplying the
Lagrangian and the generalized coordinates, at the bare minimum. If there
are any constraint equations, they can be supplied as keyword arguments.
The Lagrange multipliers are automatically generated and are equal in
number to the constraint equations. Similarly any non-conservative forces
can be supplied in an iterable (as described below and also shown in the
example) along with a ReferenceFrame. This is also discussed further in the
__init__ method.
Attributes
==========
q, u : Matrix
Matrices of the generalized coordinates and speeds
forcelist : iterable
Iterable of (Point, vector) or (ReferenceFrame, vector) tuples
describing the forces on the system.
bodies : iterable
Iterable containing the rigid bodies and particles of the system.
mass_matrix : Matrix
The system's mass matrix
forcing : Matrix
The system's forcing vector
mass_matrix_full : Matrix
The "mass matrix" for the qdot's, qdoubledot's, and the
lagrange multipliers (lam)
forcing_full : Matrix
The forcing vector for the qdot's, qdoubledot's and
lagrange multipliers (lam)
Examples
========
This is a simple example for a one degree of freedom translational
spring-mass-damper.
In this example, we first need to do the kinematics.
This involves creating generalized coordinates and their derivatives.
Then we create a point and set its velocity in a frame.
>>> from sympy.physics.mechanics import LagrangesMethod, Lagrangian
>>> from sympy.physics.mechanics import ReferenceFrame, Particle, Point
>>> from sympy.physics.mechanics import dynamicsymbols, kinetic_energy
>>> from sympy import symbols
>>> q = dynamicsymbols('q')
>>> qd = dynamicsymbols('q', 1)
>>> m, k, b = symbols('m k b')
>>> N = ReferenceFrame('N')
>>> P = Point('P')
>>> P.set_vel(N, qd * N.x)
We need to then prepare the information as required by LagrangesMethod to
generate equations of motion.
First we create the Particle, which has a point attached to it.
Following this the lagrangian is created from the kinetic and potential
energies.
Then, an iterable of nonconservative forces/torques must be constructed,
where each item is a (Point, Vector) or (ReferenceFrame, Vector) tuple,
with the Vectors representing the nonconservative forces or torques.
>>> Pa = Particle('Pa', P, m)
>>> Pa.potential_energy = k * q**2 / 2.0
>>> L = Lagrangian(N, Pa)
>>> fl = [(P, -b * qd * N.x)]
Finally we can generate the equations of motion.
First we create the LagrangesMethod object. To do this one must supply
the Lagrangian, and the generalized coordinates. The constraint equations,
the forcelist, and the inertial frame may also be provided, if relevant.
Next we generate Lagrange's equations of motion, such that:
Lagrange's equations of motion = 0.
We have the equations of motion at this point.
>>> l = LagrangesMethod(L, [q], forcelist = fl, frame = N)
>>> print(l.form_lagranges_equations())
Matrix([[b*Derivative(q(t), t) + 1.0*k*q(t) + m*Derivative(q(t), t, t)]])
We can also solve for the states using the 'rhs' method.
>>> print(l.rhs())
Matrix([[Derivative(q(t), t)], [(-b*Derivative(q(t), t) - 1.0*k*q(t))/m]])
Please refer to the docstrings on each method for more details.
"""
def __init__(self, Lagrangian, qs, forcelist=None, bodies=None, frame=None,
hol_coneqs=None, nonhol_coneqs=None):
"""Supply the following for the initialization of LagrangesMethod
Lagrangian : Sympifyable
qs : array_like
The generalized coordinates
hol_coneqs : array_like, optional
The holonomic constraint equations
nonhol_coneqs : array_like, optional
The nonholonomic constraint equations
forcelist : iterable, optional
Takes an iterable of (Point, Vector) or (ReferenceFrame, Vector)
tuples which represent the force at a point or torque on a frame.
This feature is primarily to account for the nonconservative forces
and/or moments.
bodies : iterable, optional
Takes an iterable containing the rigid bodies and particles of the
system.
frame : ReferenceFrame, optional
Supply the inertial frame. This is used to determine the
generalized forces due to non-conservative forces.
"""
self._L = Matrix([sympify(Lagrangian)])
self.eom = None
self._m_cd = Matrix() # Mass Matrix of differentiated coneqs
self._m_d = Matrix() # Mass Matrix of dynamic equations
self._f_cd = Matrix() # Forcing part of the diff coneqs
self._f_d = Matrix() # Forcing part of the dynamic equations
self.lam_coeffs = Matrix() # The coeffecients of the multipliers
forcelist = forcelist if forcelist else []
if not iterable(forcelist):
raise TypeError('Force pairs must be supplied in an iterable.')
self._forcelist = forcelist
if frame and not isinstance(frame, ReferenceFrame):
raise TypeError('frame must be a valid ReferenceFrame')
self._bodies = bodies
self.inertial = frame
self.lam_vec = Matrix()
self._term1 = Matrix()
self._term2 = Matrix()
self._term3 = Matrix()
self._term4 = Matrix()
# Creating the qs, qdots and qdoubledots
if not iterable(qs):
raise TypeError('Generalized coordinates must be an iterable')
self._q = Matrix(qs)
self._qdots = self.q.diff(dynamicsymbols._t)
self._qdoubledots = self._qdots.diff(dynamicsymbols._t)
mat_build = lambda x: Matrix(x) if x else Matrix()
hol_coneqs = mat_build(hol_coneqs)
nonhol_coneqs = mat_build(nonhol_coneqs)
self.coneqs = Matrix([hol_coneqs.diff(dynamicsymbols._t),
nonhol_coneqs])
self._hol_coneqs = hol_coneqs
def form_lagranges_equations(self):
"""Method to form Lagrange's equations of motion.
Returns a vector of equations of motion using Lagrange's equations of
the second kind.
"""
qds = self._qdots
qdd_zero = dict((i, 0) for i in self._qdoubledots)
n = len(self.q)
# Internally we represent the EOM as four terms:
# EOM = term1 - term2 - term3 - term4 = 0
# First term
self._term1 = self._L.jacobian(qds)
self._term1 = self._term1.diff(dynamicsymbols._t).T
# Second term
self._term2 = self._L.jacobian(self.q).T
# Third term
if self.coneqs:
coneqs = self.coneqs
m = len(coneqs)
# Creating the multipliers
self.lam_vec = Matrix(dynamicsymbols('lam1:' + str(m + 1)))
self.lam_coeffs = -coneqs.jacobian(qds)
self._term3 = self.lam_coeffs.T * self.lam_vec
# Extracting the coeffecients of the qdds from the diff coneqs
diffconeqs = coneqs.diff(dynamicsymbols._t)
self._m_cd = diffconeqs.jacobian(self._qdoubledots)
# The remaining terms i.e. the 'forcing' terms in diff coneqs
self._f_cd = -diffconeqs.subs(qdd_zero)
else:
self._term3 = zeros(n, 1)
# Fourth term
if self.forcelist:
N = self.inertial
self._term4 = zeros(n, 1)
for i, qd in enumerate(qds):
flist = zip(*_f_list_parser(self.forcelist, N))
self._term4[i] = sum(v.diff(qd, N) & f for (v, f) in flist)
else:
self._term4 = zeros(n, 1)
# Form the dynamic mass and forcing matrices
without_lam = self._term1 - self._term2 - self._term4
self._m_d = without_lam.jacobian(self._qdoubledots)
self._f_d = -without_lam.subs(qdd_zero)
# Form the EOM
self.eom = without_lam - self._term3
return self.eom
@property
def mass_matrix(self):
"""Returns the mass matrix, which is augmented by the Lagrange
multipliers, if necessary.
If the system is described by 'n' generalized coordinates and there are
no constraint equations then an n X n matrix is returned.
If there are 'n' generalized coordinates and 'm' constraint equations
have been supplied during initialization then an n X (n+m) matrix is
returned. The (n + m - 1)th and (n + m)th columns contain the
coefficients of the Lagrange multipliers.
"""
if self.eom is None:
raise ValueError('Need to compute the equations of motion first')
if self.coneqs:
return (self._m_d).row_join(self.lam_coeffs.T)
else:
return self._m_d
@property
def mass_matrix_full(self):
"""Augments the coefficients of qdots to the mass_matrix."""
if self.eom is None:
raise ValueError('Need to compute the equations of motion first')
n = len(self.q)
m = len(self.coneqs)
row1 = eye(n).row_join(zeros(n, n + m))
row2 = zeros(n, n).row_join(self.mass_matrix)
if self.coneqs:
row3 = zeros(m, n).row_join(self._m_cd).row_join(zeros(m, m))
return row1.col_join(row2).col_join(row3)
else:
return row1.col_join(row2)
@property
def forcing(self):
"""Returns the forcing vector from 'lagranges_equations' method."""
if self.eom is None:
raise ValueError('Need to compute the equations of motion first')
return self._f_d
@property
def forcing_full(self):
"""Augments qdots to the forcing vector above."""
if self.eom is None:
raise ValueError('Need to compute the equations of motion first')
if self.coneqs:
return self._qdots.col_join(self.forcing).col_join(self._f_cd)
else:
return self._qdots.col_join(self.forcing)
def to_linearizer(self, q_ind=None, qd_ind=None, q_dep=None, qd_dep=None):
"""Returns an instance of the Linearizer class, initiated from the
data in the LagrangesMethod class. This may be more desirable than using
the linearize class method, as the Linearizer object will allow more
efficient recalculation (i.e. about varying operating points).
Parameters
==========
q_ind, qd_ind : array_like, optional
The independent generalized coordinates and speeds.
q_dep, qd_dep : array_like, optional
The dependent generalized coordinates and speeds.
"""
# Compose vectors
t = dynamicsymbols._t
q = self.q
u = self._qdots
ud = u.diff(t)
# Get vector of lagrange multipliers
lams = self.lam_vec
mat_build = lambda x: Matrix(x) if x else Matrix()
q_i = mat_build(q_ind)
q_d = mat_build(q_dep)
u_i = mat_build(qd_ind)
u_d = mat_build(qd_dep)
# Compose general form equations
f_c = self._hol_coneqs
f_v = self.coneqs
f_a = f_v.diff(t)
f_0 = u
f_1 = -u
f_2 = self._term1
f_3 = -(self._term2 + self._term4)
f_4 = -self._term3
# Check that there are an appropriate number of independent and
# dependent coordinates
if len(q_d) != len(f_c) or len(u_d) != len(f_v):
raise ValueError(("Must supply {:} dependent coordinates, and " +
"{:} dependent speeds").format(len(f_c), len(f_v)))
if set(Matrix([q_i, q_d])) != set(q):
raise ValueError("Must partition q into q_ind and q_dep, with " +
"no extra or missing symbols.")
if set(Matrix([u_i, u_d])) != set(u):
raise ValueError("Must partition qd into qd_ind and qd_dep, " +
"with no extra or missing symbols.")
# Find all other dynamic symbols, forming the forcing vector r.
# Sort r to make it canonical.
insyms = set(Matrix([q, u, ud, lams]))
r = list(find_dynamicsymbols(f_3, insyms))
r.sort(key=default_sort_key)
# Check for any derivatives of variables in r that are also found in r.
for i in r:
if diff(i, dynamicsymbols._t) in r:
raise ValueError('Cannot have derivatives of specified \
quantities when linearizing forcing terms.')
return Linearizer(f_0, f_1, f_2, f_3, f_4, f_c, f_v, f_a, q, u, q_i,
q_d, u_i, u_d, r, lams)
def linearize(self, q_ind=None, qd_ind=None, q_dep=None, qd_dep=None,
**kwargs):
"""Linearize the equations of motion about a symbolic operating point.
If kwarg A_and_B is False (default), returns M, A, B, r for the
linearized form, M*[q', u']^T = A*[q_ind, u_ind]^T + B*r.
If kwarg A_and_B is True, returns A, B, r for the linearized form
dx = A*x + B*r, where x = [q_ind, u_ind]^T. Note that this is
computationally intensive if there are many symbolic parameters. For
this reason, it may be more desirable to use the default A_and_B=False,
returning M, A, and B. Values may then be substituted in to these
matrices, and the state space form found as
A = P.T*M.inv()*A, B = P.T*M.inv()*B, where P = Linearizer.perm_mat.
In both cases, r is found as all dynamicsymbols in the equations of
motion that are not part of q, u, q', or u'. They are sorted in
canonical form.
The operating points may be also entered using the ``op_point`` kwarg.
This takes a dictionary of {symbol: value}, or a an iterable of such
dictionaries. The values may be numberic or symbolic. The more values
you can specify beforehand, the faster this computation will run.
For more documentation, please see the ``Linearizer`` class."""
linearizer = self.to_linearizer(q_ind, qd_ind, q_dep, qd_dep)
result = linearizer.linearize(**kwargs)
return result + (linearizer.r,)
def solve_multipliers(self, op_point=None, sol_type='dict'):
"""Solves for the values of the lagrange multipliers symbolically at
the specified operating point
Parameters
==========
op_point : dict or iterable of dicts, optional
Point at which to solve at. The operating point is specified as
a dictionary or iterable of dictionaries of {symbol: value}. The
value may be numeric or symbolic itself.
sol_type : str, optional
Solution return type. Valid options are:
- 'dict': A dict of {symbol : value} (default)
- 'Matrix': An ordered column matrix of the solution
"""
# Determine number of multipliers
k = len(self.lam_vec)
if k == 0:
raise ValueError("System has no lagrange multipliers to solve for.")
# Compose dict of operating conditions
if isinstance(op_point, dict):
op_point_dict = op_point
elif iterable(op_point):
op_point_dict = {}
for op in op_point:
op_point_dict.update(op)
elif op_point is None:
op_point_dict = {}
else:
raise TypeError("op_point must be either a dictionary or an "
"iterable of dictionaries.")
# Compose the system to be solved
mass_matrix = self.mass_matrix.col_join((-self.lam_coeffs.row_join(
zeros(k, k))))
force_matrix = self.forcing.col_join(self._f_cd)
# Sub in the operating point
mass_matrix = msubs(mass_matrix, op_point_dict)
force_matrix = msubs(force_matrix, op_point_dict)
# Solve for the multipliers
sol_list = mass_matrix.LUsolve(-force_matrix)[-k:]
if sol_type == 'dict':
return dict(zip(self.lam_vec, sol_list))
elif sol_type == 'Matrix':
return Matrix(sol_list)
else:
raise ValueError("Unknown sol_type {:}.".format(sol_type))
def rhs(self, inv_method=None, **kwargs):
"""Returns equations that can be solved numerically
Parameters
==========
inv_method : str
The specific sympy inverse matrix calculation method to use. For a
list of valid methods, see
:meth:`~sympy.matrices.matrices.MatrixBase.inv`
"""
if inv_method is None:
self._rhs = self.mass_matrix_full.LUsolve(self.forcing_full)
else:
self._rhs = (self.mass_matrix_full.inv(inv_method,
try_block_diag=True) * self.forcing_full)
return self._rhs
@property
def q(self):
return self._q
@property
def u(self):
return self._qdots
@property
def bodies(self):
return self._bodies
@property
def forcelist(self):
return self._forcelist
def _initialize_kindiffeq_matrices(self, kdeqs):
"""Initialize the kinematic differential equation matrices.
Parameters
==========
kdeqs : sequence of sympy expressions
Kinematic differential equations in the form of f(u,q',q,t) where
f() = 0. The equations have to be linear in the generalized
coordinates and generalized speeds.
"""
if kdeqs:
if len(self.q) != len(kdeqs):
raise ValueError('There must be an equal number of kinematic '
'differential equations and coordinates.')
u = self.u
qdot = self._qdot
kdeqs = Matrix(kdeqs)
u_zero = {ui: 0 for ui in u}
uaux_zero = {uai: 0 for uai in self._uaux}
qdot_zero = {qdi: 0 for qdi in qdot}
# Extract the linear coefficient matrices as per the following
# equation:
#
# k_ku(q,t)*u(t) + k_kqdot(q,t)*q'(t) + f_k(q,t) = 0
#
k_ku = kdeqs.jacobian(u)
k_kqdot = kdeqs.jacobian(qdot)
f_k = kdeqs.xreplace(u_zero).xreplace(qdot_zero)
# The kinematic differential equations should be linear in both q'
# and u, so check for u and q' in the components.
dy_syms = find_dynamicsymbols(k_ku.row_join(k_kqdot).row_join(f_k))
nonlin_vars = [vari for vari in u[:] + qdot[:] if vari in dy_syms]
if nonlin_vars:
msg = ('The provided kinematic differential equations are '
'nonlinear in {}. They must be linear in the '
'generalized speeds and derivatives of the generalized '
'coordinates.')
raise ValueError(msg.format(nonlin_vars))
# Solve for q'(t) such that the coefficient matrices are now in
# this form:
#
# k_kqdot^-1*k_ku*u(t) + I*q'(t) + k_kqdot^-1*f_k = 0
#
# NOTE : Solving the kinematic differential equations here is not
# necessary and prevents the equations from being provided in fully
# implicit form.
f_k = k_kqdot.LUsolve(f_k)
k_ku = k_kqdot.LUsolve(k_ku)
k_kqdot = eye(len(qdot))
self._qdot_u_map = dict(zip(qdot, -(k_ku * u + f_k)))
self._f_k = f_k.xreplace(uaux_zero)
self._k_ku = k_ku.xreplace(uaux_zero)
self._k_kqdot = k_kqdot
else:
self._qdot_u_map = None
self._f_k = Matrix()
self._k_ku = Matrix()
self._k_kqdot = Matrix()
