def test_linearize_rolling_disc_kane():
# Symbols for time and constant parameters
t, r, m, g, v = symbols('t r m g v')
# Configuration variables and their time derivatives
q1, q2, q3, q4, q5, q6 = q = dynamicsymbols('q1:7')
q1d, q2d, q3d, q4d, q5d, q6d = qd = [qi.diff(t) for qi in q]
# Generalized speeds and their time derivatives
u = dynamicsymbols('u:6')
u1, u2, u3, u4, u5, u6 = u = dynamicsymbols('u1:7')
u1d, u2d, u3d, u4d, u5d, u6d = [ui.diff(t) for ui in u]
# Reference frames
N = ReferenceFrame('N') # Inertial frame
NO = Point('NO') # Inertial origin
A = N.orientnew('A', 'Axis', [q1, N.z]) # Yaw intermediate frame
B = A.orientnew('B', 'Axis', [q2, A.x]) # Lean intermediate frame
C = B.orientnew('C', 'Axis', [q3, B.y]) # Disc fixed frame
CO = NO.locatenew('CO', q4*N.x + q5*N.y + q6*N.z) # Disc center
# Disc angular velocity in N expressed using time derivatives of coordinates
w_c_n_qd = C.ang_vel_in(N)
w_b_n_qd = B.ang_vel_in(N)
# Inertial angular velocity and angular acceleration of disc fixed frame
C.set_ang_vel(N, u1*B.x + u2*B.y + u3*B.z)
# Disc center velocity in N expressed using time derivatives of coordinates
v_co_n_qd = CO.pos_from(NO).dt(N)
# Disc center velocity in N expressed using generalized speeds
CO.set_vel(N, u4*C.x + u5*C.y + u6*C.z)
# Disc Ground Contact Point
P = CO.locatenew('P', r*B.z)
P.v2pt_theory(CO, N, C)
# Configuration constraint
f_c = Matrix([q6 - dot(CO.pos_from(P), N.z)])
# Velocity level constraints
f_v = Matrix([dot(P.vel(N), uv) for uv in C])
# Kinematic differential equations
kindiffs = Matrix([dot(w_c_n_qd - C.ang_vel_in(N), uv) for uv in B] +
[dot(v_co_n_qd - CO.vel(N), uv) for uv in N])
qdots = solve(kindiffs, qd)
# Set angular velocity of remaining frames
B.set_ang_vel(N, w_b_n_qd.subs(qdots))
C.set_ang_acc(N, C.ang_vel_in(N).dt(B) + cross(B.ang_vel_in(N), C.ang_vel_in(N)))
# Active forces
F_CO = m*g*A.z
# Create inertia dyadic of disc C about point CO
I = (m * r**2) / 4
J = (m * r**2) / 2
I_C_CO = inertia(C, I, J, I)
Disc = RigidBody('Disc', CO, C, m, (I_C_CO, CO))
BL = [Disc]
FL = [(CO, F_CO)]
KM = KanesMethod(N, [q1, q2, q3, q4, q5], [u1, u2, u3], kd_eqs=kindiffs,
q_dependent=[q6], configuration_constraints=f_c,
u_dependent=[u4, u5, u6], velocity_constraints=f_v)
with warns_deprecated_sympy():
(fr, fr_star) = KM.kanes_equations(FL, BL)
# Test generalized form equations
linearizer = KM.to_linearizer()
assert linearizer.f_c == f_c
assert linearizer.f_v == f_v
assert linearizer.f_a == f_v.diff(t)
sol = solve(linearizer.f_0 + linearizer.f_1, qd)
for qi in qd:
assert sol[qi] == qdots[qi]
assert simplify(linearizer.f_2 + linearizer.f_3 - fr - fr_star) == Matrix([0, 0, 0])
# Perform the linearization
# Precomputed operating point
q_op = {q6: -r*cos(q2)}
u_op = {u1: 0,
u2: sin(q2)*q1d + q3d,
u3: cos(q2)*q1d,
u4: -r*(sin(q2)*q1d + q3d)*cos(q3),
u5: 0,
u6: -r*(sin(q2)*q1d + q3d)*sin(q3)}
qd_op = {q2d: 0,
q4d: -r*(sin(q2)*q1d + q3d)*cos(q1),
q5d: -r*(sin(q2)*q1d + q3d)*sin(q1),
q6d: 0}
ud_op = {u1d: 4*g*sin(q2)/(5*r) + sin(2*q2)*q1d**2/2 + 6*cos(q2)*q1d*q3d/5,
u2d: 0,
u3d: 0,
u4d: r*(sin(q2)*sin(q3)*q1d*q3d + sin(q3)*q3d**2),
u5d: r*(4*g*sin(q2)/(5*r) + sin(2*q2)*q1d**2/2 + 6*cos(q2)*q1d*q3d/5),
u6d: -r*(sin(q2)*cos(q3)*q1d*q3d + cos(q3)*q3d**2)}
A, B = linearizer.linearize(op_point=[q_op, u_op, qd_op, ud_op], A_and_B=True, simplify=True)
upright_nominal = {q1d: 0, q2: 0, m: 1, r: 1, g: 1}
# Precomputed solution
A_sol = Matrix([[0, 0, 0, 0, 0, 0, 0, 1],
[0, 0, 0, 0, 0, 1, 0, 0],
[0, 0, 0, 0, 0, 0, 1, 0],
[sin(q1)*q3d, 0, 0, 0, 0, -sin(q1), -cos(q1), 0],
[-cos(q1)*q3d, 0, 0, 0, 0, cos(q1), -sin(q1), 0],
[0, Rational(4, 5), 0, 0, 0, 0, 0, 6*q3d/5],
[0, 0, 0, 0, 0, 0, 0, 0],
[0, 0, 0, 0, 0, -2*q3d, 0, 0]])
B_sol = Matrix([])
# Check that linearization is correct
assert A.subs(upright_nominal) == A_sol
assert B.subs(upright_nominal) == B_sol
# Check eigenvalues at critical speed are all zero:
assert A.subs(upright_nominal).subs(q3d, 1/sqrt(3)).eigenvals() == {0: 8}
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
def test_linearize_pendulum_kane_nonminimal():
# Create generalized coordinates and speeds for this non-minimal realization
# q1, q2 = N.x and N.y coordinates of pendulum
# u1, u2 = N.x and N.y velocities of pendulum
q1, q2 = dynamicsymbols('q1:3')
q1d, q2d = dynamicsymbols('q1:3', level=1)
u1, u2 = dynamicsymbols('u1:3')
u1d, u2d = dynamicsymbols('u1:3', level=1)
L, m, t = symbols('L, m, t')
g = 9.8
# Compose world frame
N = ReferenceFrame('N')
pN = Point('N*')
pN.set_vel(N, 0)
# A.x is along the pendulum
theta1 = atan(q2/q1)
A = N.orientnew('A', 'axis', [theta1, N.z])
# Locate the pendulum mass
P = pN.locatenew('P1', q1*N.x + q2*N.y)
pP = Particle('pP', P, m)
# Calculate the kinematic differential equations
kde = Matrix([q1d - u1,
q2d - u2])
dq_dict = solve(kde, [q1d, q2d])
# Set velocity of point P
P.set_vel(N, P.pos_from(pN).dt(N).subs(dq_dict))
# Configuration constraint is length of pendulum
f_c = Matrix([P.pos_from(pN).magnitude() - L])
# Velocity constraint is that the velocity in the A.x direction is
# always zero (the pendulum is never getting longer).
f_v = Matrix([P.vel(N).express(A).dot(A.x)])
f_v.simplify()
# Acceleration constraints is the time derivative of the velocity constraint
f_a = f_v.diff(t)
f_a.simplify()
# Input the force resultant at P
R = m*g*N.x
# Derive the equations of motion using the KanesMethod class.
KM = KanesMethod(N, q_ind=[q2], u_ind=[u2], q_dependent=[q1],
u_dependent=[u1], configuration_constraints=f_c,
velocity_constraints=f_v, acceleration_constraints=f_a, kd_eqs=kde)
with warns_deprecated_sympy():
(fr, frstar) = KM.kanes_equations([(P, R)], [pP])
# Set the operating point to be straight down, and non-moving
q_op = {q1: L, q2: 0}
u_op = {u1: 0, u2: 0}
ud_op = {u1d: 0, u2d: 0}
A, B, inp_vec = KM.linearize(op_point=[q_op, u_op, ud_op], A_and_B=True,
simplify=True)
assert A.expand() == Matrix([[0, 1], [-9.8/L, 0]])
assert B == Matrix([])
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
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 KanesMethod(object):
"""Kane's method object.
This object is used to do the "book-keeping" as you go through and form
equations of motion in the way Kane presents in:
Kane, T., Levinson, D. Dynamics Theory and Applications. 1985 McGraw-Hill
The attributes are for equations in the form [M] udot = forcing.
Attributes
==========
q, u : Matrix
Matrices of the generalized coordinates and speeds
bodylist : iterable
Iterable of Point and RigidBody objects in the system.
forcelist : iterable
Iterable of (Point, vector) or (ReferenceFrame, vector) tuples
describing the forces on the system.
auxiliary : Matrix
If applicable, the set of auxiliary Kane's
equations used to solve for non-contributing
forces.
mass_matrix : Matrix
The system's mass matrix
forcing : Matrix
The system's forcing vector
mass_matrix_full : Matrix
The "mass matrix" for the u's and q's
forcing_full : Matrix
The "forcing vector" for the u's and q's
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 speeds and coordinates and their
derivatives.
Then we create a point and set its velocity in a frame.
>>> from sympy import symbols
>>> from sympy.physics.mechanics import dynamicsymbols, ReferenceFrame
>>> from sympy.physics.mechanics import Point, Particle, KanesMethod
>>> q, u = dynamicsymbols('q u')
>>> qd, ud = dynamicsymbols('q u', 1)
>>> m, c, k = symbols('m c k')
>>> N = ReferenceFrame('N')
>>> P = Point('P')
>>> P.set_vel(N, u * N.x)
Next we need to arrange/store information in the way that KanesMethod
requires. The kinematic differential equations need to be stored in a
dict. A list of forces/torques must be constructed, where each entry in
the list is a (Point, Vector) or (ReferenceFrame, Vector) tuple, where the
Vectors represent the Force or Torque.
Next a particle needs to be created, and it needs to have a point and mass
assigned to it.
Finally, a list of all bodies and particles needs to be created.
>>> kd = [qd - u]
>>> FL = [(P, (-k * q - c * u) * N.x)]
>>> pa = Particle('pa', P, m)
>>> BL = [pa]
Finally we can generate the equations of motion.
First we create the KanesMethod object and supply an inertial frame,
coordinates, generalized speeds, and the kinematic differential equations.
Additional quantities such as configuration and motion constraints,
dependent coordinates and speeds, and auxiliary speeds are also supplied
here (see the online documentation).
Next we form FR* and FR to complete: Fr + Fr* = 0.
We have the equations of motion at this point.
It makes sense to rearrnge them though, so we calculate the mass matrix and
the forcing terms, for E.o.M. in the form: [MM] udot = forcing, where MM is
the mass matrix, udot is a vector of the time derivatives of the
generalized speeds, and forcing is a vector representing "forcing" terms.
>>> KM = KanesMethod(N, q_ind=[q], u_ind=[u], kd_eqs=kd)
>>> (fr, frstar) = KM.kanes_equations(BL, FL)
>>> MM = KM.mass_matrix
>>> forcing = KM.forcing
>>> rhs = MM.inv() * forcing
>>> rhs
Matrix([[(-c*u(t) - k*q(t))/m]])
>>> KM.linearize(A_and_B=True, new_method=True)[0]
Matrix([
[ 0, 1],
[-k/m, -c/m]])
Please look at the documentation pages for more information on how to
perform linearization and how to deal with dependent coordinates & speeds,
and how do deal with bringing non-contributing forces into evidence.
"""
def __init__(self,
frame,
q_ind,
u_ind,
kd_eqs=None,
q_dependent=None,
configuration_constraints=None,
u_dependent=None,
velocity_constraints=None,
acceleration_constraints=None,
u_auxiliary=None):
"""Please read the online documentation. """
if not isinstance(frame, ReferenceFrame):
raise TypeError('An intertial ReferenceFrame must be supplied')
self._inertial = frame
self._fr = None
self._frstar = None
self._forcelist = None
self._bodylist = None
self._initialize_vectors(q_ind, q_dependent, u_ind, u_dependent,
u_auxiliary)
self._initialize_kindiffeq_matrices(kd_eqs)
self._initialize_constraint_matrices(configuration_constraints,
velocity_constraints,
acceleration_constraints)
def _initialize_vectors(self, q_ind, q_dep, u_ind, u_dep, u_aux):
"""Initialize the coordinate and speed vectors."""
none_handler = lambda x: Matrix(x) if x else Matrix()
# Initialize generalized coordinates
q_dep = none_handler(q_dep)
if not iterable(q_ind):
raise TypeError('Generalized coordinates must be an iterable.')
if not iterable(q_dep):
raise TypeError('Dependent coordinates must be an iterable.')
q_ind = Matrix(q_ind)
self._qdep = q_dep
self._q = Matrix([q_ind, q_dep])
self._qdot = self.q.diff(dynamicsymbols._t)
# Initialize generalized speeds
u_dep = none_handler(u_dep)
if not iterable(u_ind):
raise TypeError('Generalized speeds must be an iterable.')
if not iterable(u_dep):
raise TypeError('Dependent speeds must be an iterable.')
u_ind = Matrix(u_ind)
self._udep = u_dep
self._u = Matrix([u_ind, u_dep])
self._udot = self.u.diff(dynamicsymbols._t)
self._uaux = none_handler(u_aux)
def _initialize_constraint_matrices(self, config, vel, acc):
"""Initializes constraint matrices."""
# Define vector dimensions
o = len(self.u)
m = len(self._udep)
p = o - m
none_handler = lambda x: Matrix(x) if x else Matrix()
# Initialize configuration constraints
config = none_handler(config)
if len(self._qdep) != len(config):
raise ValueError('There must be an equal number of dependent '
'coordinates and configuration constraints.')
self._f_h = none_handler(config)
# Initialize velocity and acceleration constraints
vel = none_handler(vel)
acc = none_handler(acc)
if len(vel) != m:
raise ValueError('There must be an equal number of dependent '
'speeds and velocity constraints.')
if acc and (len(acc) != m):
raise ValueError('There must be an equal number of dependent '
'speeds and acceleration constraints.')
if vel:
u_zero = dict((i, 0) for i in self.u)
udot_zero = dict((i, 0) for i in self._udot)
# When calling kanes_equations, another class instance will be
# created if auxiliary u's are present. In this case, the
# computation of kinetic differential equation matrices will be
# skipped as this was computed during the original KanesMethod
# object, and the qd_u_map will not be available.
if self._qdot_u_map is not None:
vel = msubs(vel, self._qdot_u_map)
self._f_nh = msubs(vel, u_zero)
self._k_nh = (vel - self._f_nh).jacobian(self.u)
# If no acceleration constraints given, calculate them.
if not acc:
self._f_dnh = (self._k_nh.diff(dynamicsymbols._t) * self.u +
self._f_nh.diff(dynamicsymbols._t))
self._k_dnh = self._k_nh
else:
if self._qdot_u_map is not None:
acc = msubs(acc, self._qdot_u_map)
self._f_dnh = msubs(acc, udot_zero)
self._k_dnh = (acc - self._f_dnh).jacobian(self._udot)
# Form of non-holonomic constraints is B*u + C = 0.
# We partition B into independent and dependent columns:
# Ars is then -B_dep.inv() * B_ind, and it relates dependent speeds
# to independent speeds as: udep = Ars*uind, neglecting the C term.
B_ind = self._k_nh[:, :p]
B_dep = self._k_nh[:, p:o]
self._Ars = -B_dep.LUsolve(B_ind)
else:
self._f_nh = Matrix()
self._k_nh = Matrix()
self._f_dnh = Matrix()
self._k_dnh = Matrix()
self._Ars = Matrix()
def _initialize_kindiffeq_matrices(self, kdeqs):
"""Initialize the kinematic differential equation matrices."""
if kdeqs:
if len(self.q) != len(kdeqs):
raise ValueError('There must be an equal number of kinematic '
'differential equations and coordinates.')
kdeqs = Matrix(kdeqs)
u = self.u
qdot = self._qdot
# Dictionaries setting things to zero
u_zero = dict((i, 0) for i in u)
uaux_zero = dict((i, 0) for i in self._uaux)
qdot_zero = dict((i, 0) for i in qdot)
f_k = msubs(kdeqs, u_zero, qdot_zero)
k_ku = (msubs(kdeqs, qdot_zero) - f_k).jacobian(u)
k_kqdot = (msubs(kdeqs, u_zero) - f_k).jacobian(qdot)
f_k = k_kqdot.LUsolve(f_k)
k_ku = k_kqdot.LUsolve(k_ku)
k_kqdot = eye(len(qdot))
self._qdot_u_map = solve_linear_system_LU(
Matrix([k_kqdot.T, -(k_ku * u + f_k).T]).T, qdot)
self._f_k = msubs(f_k, uaux_zero)
self._k_ku = msubs(k_ku, 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()
def _form_fr(self, fl):
"""Form the generalized active force."""
if fl != None and (len(fl) == 0 or not iterable(fl)):
raise ValueError('Force pairs must be supplied in an '
'non-empty iterable or None.')
N = self._inertial
# pull out relevant velocities for constructing partial velocities
vel_list, f_list = _f_list_parser(fl, N)
vel_list = [msubs(i, self._qdot_u_map) for i in vel_list]
# Fill Fr with dot product of partial velocities and forces
o = len(self.u)
b = len(f_list)
FR = zeros(o, 1)
partials = partial_velocity(vel_list, self.u, N)
for i in range(o):
FR[i] = sum(partials[j][i] & f_list[j] for j in range(b))
# In case there are dependent speeds
if self._udep:
p = o - len(self._udep)
FRtilde = FR[:p, 0]
FRold = FR[p:o, 0]
FRtilde += self._Ars.T * FRold
FR = FRtilde
self._forcelist = fl
self._fr = FR
return FR
def _form_frstar(self, bl):
"""Form the generalized inertia force."""
if not iterable(bl):
raise TypeError('Bodies must be supplied in an iterable.')
t = dynamicsymbols._t
N = self._inertial
# Dicts setting things to zero
udot_zero = dict((i, 0) for i in self._udot)
uaux_zero = dict((i, 0) for i in self._uaux)
uauxdot = [diff(i, t) for i in self._uaux]
uauxdot_zero = dict((i, 0) for i in uauxdot)
# Dictionary of q' and q'' to u and u'
q_ddot_u_map = dict(
(k.diff(t), v.diff(t)) for (k, v) in self._qdot_u_map.items())
q_ddot_u_map.update(self._qdot_u_map)
# Fill up the list of partials: format is a list with num elements
# equal to number of entries in body list. Each of these elements is a
# list - either of length 1 for the translational components of
# particles or of length 2 for the translational and rotational
# components of rigid bodies. The inner most list is the list of
# partial velocities.
def get_partial_velocity(body):
if isinstance(body, RigidBody):
vlist = [body.masscenter.vel(N), body.frame.ang_vel_in(N)]
elif isinstance(body, Particle):
vlist = [
body.point.vel(N),
]
else:
raise TypeError('The body list may only contain either '
'RigidBody or Particle as list elements.')
v = [msubs(vel, self._qdot_u_map) for vel in vlist]
return partial_velocity(v, self.u, N)
partials = [get_partial_velocity(body) for body in bl]
# Compute fr_star in two components:
# fr_star = -(MM*u' + nonMM)
o = len(self.u)
MM = zeros(o, o)
nonMM = zeros(o, 1)
zero_uaux = lambda expr: msubs(expr, uaux_zero)
zero_udot_uaux = lambda expr: msubs(msubs(expr, udot_zero), uaux_zero)
for i, body in enumerate(bl):
if isinstance(body, RigidBody):
M = zero_uaux(body.mass)
I = zero_uaux(body.central_inertia)
vel = zero_uaux(body.masscenter.vel(N))
omega = zero_uaux(body.frame.ang_vel_in(N))
acc = zero_udot_uaux(body.masscenter.acc(N))
inertial_force = (M.diff(t) * vel + M * acc)
inertial_torque = zero_uaux(
(I.dt(body.frame) & omega) +
msubs(I & body.frame.ang_acc_in(N), udot_zero) +
(omega ^ (I & omega)))
for j in range(o):
tmp_vel = zero_uaux(partials[i][0][j])
tmp_ang = zero_uaux(I & partials[i][1][j])
for k in range(o):
# translational
MM[j, k] += M * (tmp_vel & partials[i][0][k])
# rotational
MM[j, k] += (tmp_ang & partials[i][1][k])
nonMM[j] += inertial_force & partials[i][0][j]
nonMM[j] += inertial_torque & partials[i][1][j]
else:
M = zero_uaux(body.mass)
vel = zero_uaux(body.point.vel(N))
acc = zero_udot_uaux(body.point.acc(N))
inertial_force = (M.diff(t) * vel + M * acc)
for j in range(o):
temp = zero_uaux(partials[i][0][j])
for k in range(o):
MM[j, k] += M * (temp & partials[i][0][k])
nonMM[j] += inertial_force & partials[i][0][j]
# Compose fr_star out of MM and nonMM
MM = zero_uaux(msubs(MM, q_ddot_u_map))
nonMM = msubs(msubs(nonMM, q_ddot_u_map), udot_zero, uauxdot_zero,
uaux_zero)
fr_star = -(MM * msubs(Matrix(self._udot), uauxdot_zero) + nonMM)
# If there are dependent speeds, we need to find fr_star_tilde
if self._udep:
p = o - len(self._udep)
fr_star_ind = fr_star[:p, 0]
fr_star_dep = fr_star[p:o, 0]
fr_star = fr_star_ind + (self._Ars.T * fr_star_dep)
# Apply the same to MM
MMi = MM[:p, :]
MMd = MM[p:o, :]
MM = MMi + (self._Ars.T * MMd)
self._bodylist = bl
self._frstar = fr_star
self._k_d = MM
self._f_d = -msubs(self._fr + self._frstar, udot_zero)
return fr_star
def to_linearizer(self):
"""Returns an instance of the Linearizer class, initiated from the
data in the KanesMethod 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)."""
if (self._fr is None) or (self._frstar is None):
raise ValueError('Need to compute Fr, Fr* first.')
# Get required equation components. The Kane's method class breaks
# these into pieces. Need to reassemble
f_c = self._f_h
if self._f_nh and self._k_nh:
f_v = self._f_nh + self._k_nh * Matrix(self.u)
else:
f_v = Matrix()
if self._f_dnh and self._k_dnh:
f_a = self._f_dnh + self._k_dnh * Matrix(self._udot)
else:
f_a = Matrix()
# Dicts to sub to zero, for splitting up expressions
u_zero = dict((i, 0) for i in self.u)
ud_zero = dict((i, 0) for i in self._udot)
qd_zero = dict((i, 0) for i in self._qdot)
qd_u_zero = dict((i, 0) for i in Matrix([self._qdot, self.u]))
# Break the kinematic differential eqs apart into f_0 and f_1
f_0 = msubs(self._f_k, u_zero) + self._k_kqdot * Matrix(self._qdot)
f_1 = msubs(self._f_k, qd_zero) + self._k_ku * Matrix(self.u)
# Break the dynamic differential eqs into f_2 and f_3
f_2 = msubs(self._frstar, qd_u_zero)
f_3 = msubs(self._frstar, ud_zero) + self._fr
f_4 = zeros(len(f_2), 1)
# Get the required vector components
q = self.q
u = self.u
if self._qdep:
q_i = q[:-len(self._qdep)]
else:
q_i = q
q_d = self._qdep
if self._udep:
u_i = u[:-len(self._udep)]
else:
u_i = u
u_d = self._udep
# Form dictionary to set auxiliary speeds & their derivatives to 0.
uaux = self._uaux
uauxdot = uaux.diff(dynamicsymbols._t)
uaux_zero = dict((i, 0) for i in Matrix([uaux, uauxdot]))
# Checking for dynamic symbols outside the dynamic differential
# equations; throws error if there is.
sym_list = set(Matrix([q, self._qdot, u, self._udot, uaux, uauxdot]))
if any(
find_dynamicsymbols(i, sym_list) for i in [
self._k_kqdot, self._k_ku, self._f_k, self._k_dnh,
self._f_dnh, self._k_d
]):
raise ValueError('Cannot have dynamicsymbols outside dynamic \
forcing vector.')
# Find all other dynamic symbols, forming the forcing vector r.
# Sort r to make it canonical.
r = list(find_dynamicsymbols(msubs(self._f_d, uaux_zero), sym_list))
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)
def linearize(self, **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.
As part of the deprecation cycle, the new method will not be used unless
the kwarg ``new_method`` is set to True. If the kwarg is missing, or set
to false, the old linearization method will be used. After next release
the need for this kwarg will be removed.
For more documentation, please see the ``Linearizer`` class."""
if 'new_method' not in kwargs or not kwargs['new_method']:
# User is still using old code.
SymPyDeprecationWarning(
'The linearize class method has changed '
'to a new interface, the old method is deprecated. To '
'use the new method, set the kwarg `new_method=True`. '
'For more information, read the docstring '
'of `linearize`.').warn()
return self._old_linearize()
# Remove the new method flag, before passing kwargs to linearize
kwargs.pop('new_method')
linearizer = self.to_linearizer()
result = linearizer.linearize(**kwargs)
return result + (linearizer.r, )
def _old_linearize(self):
"""Old method to linearize the equations of motion. Returns a tuple of
(f_lin_A, f_lin_B, y) for forming [M]qudot = [f_lin_A]qu + [f_lin_B]y.
Deprecated in favor of new method using Linearizer class. Please change
your code to use the new `linearize` method."""
if (self._fr is None) or (self._frstar is None):
raise ValueError('Need to compute Fr, Fr* first.')
# Note that this is now unneccessary, and it should never be
# encountered; I still think it should be in here in case the user
# manually sets these matrices incorrectly.
for i in self.q:
if self._k_kqdot.diff(i) != 0 * self._k_kqdot:
raise ValueError('Matrix K_kqdot must not depend on any q.')
t = dynamicsymbols._t
uaux = self._uaux
uauxdot = [diff(i, t) for i in uaux]
# dictionary of auxiliary speeds & derivatives which are equal to zero
subdict = dict(
zip(uaux[:] + uauxdot[:], [0] * (len(uaux) + len(uauxdot))))
# Checking for dynamic symbols outside the dynamic differential
# equations; throws error if there is.
insyms = set(self.q[:] + self._qdot[:] + self.u[:] + self._udot[:] +
uaux[:] + uauxdot)
if any(
find_dynamicsymbols(i, insyms) for i in [
self._k_kqdot, self._k_ku, self._f_k, self._k_dnh,
self._f_dnh, self._k_d
]):
raise ValueError('Cannot have dynamicsymbols outside dynamic \
forcing vector.')
other_dyns = list(
find_dynamicsymbols(msubs(self._f_d, subdict), insyms))
# make it canonically ordered so the jacobian is canonical
other_dyns.sort(key=default_sort_key)
for i in other_dyns:
if diff(i, dynamicsymbols._t) in other_dyns:
raise ValueError('Cannot have derivatives of specified '
'quantities when linearizing forcing terms.')
o = len(self.u) # number of speeds
n = len(self.q) # number of coordinates
l = len(self._qdep) # number of configuration constraints
m = len(self._udep) # number of motion constraints
qi = Matrix(self.q[:n - l]) # independent coords
qd = Matrix(self.q[n - l:n]) # dependent coords; could be empty
ui = Matrix(self.u[:o - m]) # independent speeds
ud = Matrix(self.u[o - m:o]) # dependent speeds; could be empty
qdot = Matrix(self._qdot) # time derivatives of coordinates
# with equations in the form MM udot = forcing, expand that to:
# MM_full [q,u].T = forcing_full. This combines coordinates and
# speeds together for the linearization, which is necessary for the
# linearization process, due to dependent coordinates. f1 is the rows
# from the kinematic differential equations, f2 is the rows from the
# dynamic differential equations (and differentiated non-holonomic
# constraints).
f1 = self._k_ku * Matrix(self.u) + self._f_k
f2 = self._f_d
# Only want to do this if these matrices have been filled in, which
# occurs when there are dependent speeds
if m != 0:
f2 = self._f_d.col_join(self._f_dnh)
fnh = self._f_nh + self._k_nh * Matrix(self.u)
f1 = msubs(f1, subdict)
f2 = msubs(f2, subdict)
fh = msubs(self._f_h, subdict)
fku = msubs(self._k_ku * Matrix(self.u), subdict)
fkf = msubs(self._f_k, subdict)
# In the code below, we are applying the chain rule by hand on these
# things. All the matrices have been changed into vectors (by
# multiplying the dynamic symbols which it is paired with), so we can
# take the jacobian of them. The basic operation is take the jacobian
# of the f1, f2 vectors wrt all of the q's and u's. f1 is a function of
# q, u, and t; f2 is a function of q, qdot, u, and t. In the code
# below, we are not considering perturbations in t. So if f1 is a
# function of the q's, u's but some of the q's or u's could be
# dependent on other q's or u's (qd's might be dependent on qi's, ud's
# might be dependent on ui's or qi's), so what we do is take the
# jacobian of the f1 term wrt qi's and qd's, the jacobian wrt the qd's
# gets multiplied by the jacobian of qd wrt qi, this is extended for
# the ud's as well. dqd_dqi is computed by taking a taylor expansion of
# the holonomic constraint equations about q*, treating q* - q as dq,
# separating into dqd (depedent q's) and dqi (independent q's) and the
# rearranging for dqd/dqi. This is again extended for the speeds.
# First case: configuration and motion constraints
if (l != 0) and (m != 0):
fh_jac_qi = fh.jacobian(qi)
fh_jac_qd = fh.jacobian(qd)
fnh_jac_qi = fnh.jacobian(qi)
fnh_jac_qd = fnh.jacobian(qd)
fnh_jac_ui = fnh.jacobian(ui)
fnh_jac_ud = fnh.jacobian(ud)
fku_jac_qi = fku.jacobian(qi)
fku_jac_qd = fku.jacobian(qd)
fku_jac_ui = fku.jacobian(ui)
fku_jac_ud = fku.jacobian(ud)
fkf_jac_qi = fkf.jacobian(qi)
fkf_jac_qd = fkf.jacobian(qd)
f1_jac_qi = f1.jacobian(qi)
f1_jac_qd = f1.jacobian(qd)
f1_jac_ui = f1.jacobian(ui)
f1_jac_ud = f1.jacobian(ud)
f2_jac_qi = f2.jacobian(qi)
f2_jac_qd = f2.jacobian(qd)
f2_jac_ui = f2.jacobian(ui)
f2_jac_ud = f2.jacobian(ud)
f2_jac_qdot = f2.jacobian(qdot)
dqd_dqi = -fh_jac_qd.LUsolve(fh_jac_qi)
dud_dqi = fnh_jac_ud.LUsolve(fnh_jac_qd * dqd_dqi - fnh_jac_qi)
dud_dui = -fnh_jac_ud.LUsolve(fnh_jac_ui)
dqdot_dui = -self._k_kqdot.inv() * (fku_jac_ui +
fku_jac_ud * dud_dui)
dqdot_dqi = -self._k_kqdot.inv() * (
fku_jac_qi + fkf_jac_qi +
(fku_jac_qd + fkf_jac_qd) * dqd_dqi + fku_jac_ud * dud_dqi)
f1_q = f1_jac_qi + f1_jac_qd * dqd_dqi + f1_jac_ud * dud_dqi
f1_u = f1_jac_ui + f1_jac_ud * dud_dui
f2_q = (f2_jac_qi + f2_jac_qd * dqd_dqi + f2_jac_qdot * dqdot_dqi +
f2_jac_ud * dud_dqi)
f2_u = f2_jac_ui + f2_jac_ud * dud_dui + f2_jac_qdot * dqdot_dui
# Second case: configuration constraints only
elif l != 0:
dqd_dqi = -fh.jacobian(qd).LUsolve(fh.jacobian(qi))
dqdot_dui = -self._k_kqdot.inv() * fku.jacobian(ui)
dqdot_dqi = -self._k_kqdot.inv() * (
fku.jacobian(qi) + fkf.jacobian(qi) +
(fku.jacobian(qd) + fkf.jacobian(qd)) * dqd_dqi)
f1_q = (f1.jacobian(qi) + f1.jacobian(qd) * dqd_dqi)
f1_u = f1.jacobian(ui)
f2_jac_qdot = f2.jacobian(qdot)
f2_q = (f2.jacobian(qi) + f2.jacobian(qd) * dqd_dqi +
f2.jac_qdot * dqdot_dqi)
f2_u = f2.jacobian(ui) + f2_jac_qdot * dqdot_dui
# Third case: motion constraints only
elif m != 0:
dud_dqi = fnh.jacobian(ud).LUsolve(-fnh.jacobian(qi))
dud_dui = -fnh.jacobian(ud).LUsolve(fnh.jacobian(ui))
dqdot_dui = -self._k_kqdot.inv() * (fku.jacobian(ui) +
fku.jacobian(ud) * dud_dui)
dqdot_dqi = -self._k_kqdot.inv() * (fku.jacobian(qi) +
fkf.jacobian(qi) +
fku.jacobian(ud) * dud_dqi)
f1_jac_ud = f1.jacobian(ud)
f2_jac_qdot = f2.jacobian(qdot)
f2_jac_ud = f2.jacobian(ud)
f1_q = f1.jacobian(qi) + f1_jac_ud * dud_dqi
f1_u = f1.jacobian(ui) + f1_jac_ud * dud_dui
f2_q = (f2.jacobian(qi) + f2_jac_qdot * dqdot_dqi +
f2_jac_ud * dud_dqi)
f2_u = (f2.jacobian(ui) + f2_jac_ud * dud_dui +
f2_jac_qdot * dqdot_dui)
# Fourth case: No constraints
else:
dqdot_dui = -self._k_kqdot.inv() * fku.jacobian(ui)
dqdot_dqi = -self._k_kqdot.inv() * (fku.jacobian(qi) +
fkf.jacobian(qi))
f1_q = f1.jacobian(qi)
f1_u = f1.jacobian(ui)
f2_jac_qdot = f2.jacobian(qdot)
f2_q = f2.jacobian(qi) + f2_jac_qdot * dqdot_dqi
f2_u = f2.jacobian(ui) + f2_jac_qdot * dqdot_dui
f_lin_A = -(f1_q.row_join(f1_u)).col_join(f2_q.row_join(f2_u))
if other_dyns:
f1_oths = f1.jacobian(other_dyns)
f2_oths = f2.jacobian(other_dyns)
f_lin_B = -f1_oths.col_join(f2_oths)
else:
f_lin_B = Matrix()
return (f_lin_A, f_lin_B, Matrix(other_dyns))
def kanes_equations(self, bodies, loads=None):
""" Method to form Kane's equations, Fr + Fr* = 0.
Returns (Fr, Fr*). In the case where auxiliary generalized speeds are
present (say, s auxiliary speeds, o generalized speeds, and m motion
constraints) the length of the returned vectors will be o - m + s in
length. The first o - m equations will be the constrained Kane's
equations, then the s auxiliary Kane's equations. These auxiliary
equations can be accessed with the auxiliary_eqs().
Parameters
==========
bodies : iterable
An iterable of all RigidBody's and Particle's in the system.
A system must have at least one body.
loads : iterable
Takes in an iterable of (Particle, Vector) or (ReferenceFrame, Vector)
tuples which represent the force at a point or torque on a frame.
Must be either a non-empty iterable of tuples or None which corresponds
to a system with no constraints.
"""
if (bodies is None and loads != None) or isinstance(bodies[0], tuple):
# This switches the order if they use the old way.
bodies, loads = loads, bodies
SymPyDeprecationWarning(
value='The API for kanes_equations() has changed such '
'that the loads (forces and torques) are now the second argument '
'and is optional with None being the default.',
feature='The kanes_equation() argument order',
useinstead=
'switched argument order to update your code, For example: '
'kanes_equations(loads, bodies) > kanes_equations(bodies, loads).',
issue=10945,
deprecated_since_version="1.1").warn()
if not self._k_kqdot:
raise AttributeError(
'Create an instance of KanesMethod with '
'kinematic differential equations to use this method.')
fr = self._form_fr(loads)
frstar = self._form_frstar(bodies)
if self._uaux:
if not self._udep:
km = KanesMethod(self._inertial,
self.q,
self._uaux,
u_auxiliary=self._uaux)
else:
km = KanesMethod(self._inertial,
self.q,
self._uaux,
u_auxiliary=self._uaux,
u_dependent=self._udep,
velocity_constraints=(self._k_nh * self.u +
self._f_nh))
km._qdot_u_map = self._qdot_u_map
self._km = km
fraux = km._form_fr(loads)
frstaraux = km._form_frstar(bodies)
self._aux_eq = fraux + frstaraux
self._fr = fr.col_join(fraux)
self._frstar = frstar.col_join(frstaraux)
return (self._fr, self._frstar)
def rhs(self, inv_method=None):
"""Returns the system's equations of motion in first order form. The
output is the right hand side of::
x' = |q'| =: f(q, u, r, p, t)
|u'|
The right hand side is what is needed by most numerical ODE
integrators.
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`
"""
rhs = zeros(len(self.q) + len(self.u), c=1)
kdes = self.kindiffdict()
for i, q_i in enumerate(self.q):
rhs[i] = kdes[q_i.diff()]
if inv_method is None:
rhs[len(self.q):, 0] = self.mass_matrix.LUsolve(self.forcing)
else:
rhs[len(self.q):,
0] = (self.mass_matrix.inv(inv_method, try_block_diag=True) *
self.forcing)
return rhs
def kindiffdict(self):
"""Returns a dictionary mapping q' to u."""
if not self._qdot_u_map:
raise AttributeError(
'Create an instance of KanesMethod with '
'kinematic differential equations to use this method.')
return self._qdot_u_map
@property
def auxiliary_eqs(self):
"""A matrix containing the auxiliary equations."""
if not self._fr or not self._frstar:
raise ValueError('Need to compute Fr, Fr* first.')
if not self._uaux:
raise ValueError('No auxiliary speeds have been declared.')
return self._aux_eq
@property
def mass_matrix(self):
"""The mass matrix of the system."""
if not self._fr or not self._frstar:
raise ValueError('Need to compute Fr, Fr* first.')
return Matrix([self._k_d, self._k_dnh])
@property
def mass_matrix_full(self):
"""The mass matrix of the system, augmented by the kinematic
differential equations."""
if not self._fr or not self._frstar:
raise ValueError('Need to compute Fr, Fr* first.')
o = len(self.u)
n = len(self.q)
return ((self._k_kqdot).row_join(zeros(n, o))).col_join(
(zeros(o, n)).row_join(self.mass_matrix))
@property
def forcing(self):
"""The forcing vector of the system."""
if not self._fr or not self._frstar:
raise ValueError('Need to compute Fr, Fr* first.')
return -Matrix([self._f_d, self._f_dnh])
@property
def forcing_full(self):
"""The forcing vector of the system, augmented by the kinematic
differential equations."""
if not self._fr or not self._frstar:
raise ValueError('Need to compute Fr, Fr* first.')
f1 = self._k_ku * Matrix(self.u) + self._f_k
return -Matrix([f1, self._f_d, self._f_dnh])
@property
def q(self):
return self._q
@property
def u(self):
return self._u
@property
def bodylist(self):
return self._bodylist
@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
def test_linearize_pendulum_kane_nonminimal():
# Create generalized coordinates and speeds for this non-minimal realization
# q1, q2 = N.x and N.y coordinates of pendulum
# u1, u2 = N.x and N.y velocities of pendulum
q1, q2 = dynamicsymbols('q1:3')
q1d, q2d = dynamicsymbols('q1:3', level=1)
u1, u2 = dynamicsymbols('u1:3')
u1d, u2d = dynamicsymbols('u1:3', level=1)
L, m, t = symbols('L, m, t')
g = 9.8
# Compose world frame
N = ReferenceFrame('N')
pN = Point('N*')
pN.set_vel(N, 0)
# A.x is along the pendulum
theta1 = atan(q2/q1)
A = N.orientnew('A', 'axis', [theta1, N.z])
# Locate the pendulum mass
P = pN.locatenew('P1', q1*N.x + q2*N.y)
pP = Particle('pP', P, m)
# Calculate the kinematic differential equations
kde = Matrix([q1d - u1,
q2d - u2])
dq_dict = solve(kde, [q1d, q2d])
# Set velocity of point P
P.set_vel(N, P.pos_from(pN).dt(N).subs(dq_dict))
# Configuration constraint is length of pendulum
f_c = Matrix([P.pos_from(pN).magnitude() - L])
# Velocity constraint is that the velocity in the A.x direction is
# always zero (the pendulum is never getting longer).
f_v = Matrix([P.vel(N).express(A).dot(A.x)])
f_v.simplify()
# Acceleration constraints is the time derivative of the velocity constraint
f_a = f_v.diff(t)
f_a.simplify()
# Input the force resultant at P
R = m*g*N.x
# Derive the equations of motion using the KanesMethod class.
KM = KanesMethod(N, q_ind=[q2], u_ind=[u2], q_dependent=[q1],
u_dependent=[u1], configuration_constraints=f_c,
velocity_constraints=f_v, acceleration_constraints=f_a, kd_eqs=kde)
with warnings.catch_warnings():
warnings.filterwarnings("ignore", category=SymPyDeprecationWarning)
(fr, frstar) = KM.kanes_equations([(P, R)], [pP])
# Set the operating point to be straight down, and non-moving
q_op = {q1: L, q2: 0}
u_op = {u1: 0, u2: 0}
ud_op = {u1d: 0, u2d: 0}
A, B, inp_vec = KM.linearize(op_point=[q_op, u_op, ud_op], A_and_B=True,
new_method=True, simplify=True)
assert A.expand() == Matrix([[0, 1], [-9.8/L, 0]])
assert B == Matrix([])
def test_linearize_rolling_disc_kane():
# Symbols for time and constant parameters
t, r, m, g, v = symbols('t r m g v')
# Configuration variables and their time derivatives
q1, q2, q3, q4, q5, q6 = q = dynamicsymbols('q1:7')
q1d, q2d, q3d, q4d, q5d, q6d = qd = [qi.diff(t) for qi in q]
# Generalized speeds and their time derivatives
u = dynamicsymbols('u:6')
u1, u2, u3, u4, u5, u6 = u = dynamicsymbols('u1:7')
u1d, u2d, u3d, u4d, u5d, u6d = [ui.diff(t) for ui in u]
# Reference frames
N = ReferenceFrame('N') # Inertial frame
NO = Point('NO') # Inertial origin
A = N.orientnew('A', 'Axis', [q1, N.z]) # Yaw intermediate frame
B = A.orientnew('B', 'Axis', [q2, A.x]) # Lean intermediate frame
C = B.orientnew('C', 'Axis', [q3, B.y]) # Disc fixed frame
CO = NO.locatenew('CO', q4*N.x + q5*N.y + q6*N.z) # Disc center
# Disc angular velocity in N expressed using time derivatives of coordinates
w_c_n_qd = C.ang_vel_in(N)
w_b_n_qd = B.ang_vel_in(N)
# Inertial angular velocity and angular acceleration of disc fixed frame
C.set_ang_vel(N, u1*B.x + u2*B.y + u3*B.z)
# Disc center velocity in N expressed using time derivatives of coordinates
v_co_n_qd = CO.pos_from(NO).dt(N)
# Disc center velocity in N expressed using generalized speeds
CO.set_vel(N, u4*C.x + u5*C.y + u6*C.z)
# Disc Ground Contact Point
P = CO.locatenew('P', r*B.z)
P.v2pt_theory(CO, N, C)
# Configuration constraint
f_c = Matrix([q6 - dot(CO.pos_from(P), N.z)])
# Velocity level constraints
f_v = Matrix([dot(P.vel(N), uv) for uv in C])
# Kinematic differential equations
kindiffs = Matrix([dot(w_c_n_qd - C.ang_vel_in(N), uv) for uv in B] +
[dot(v_co_n_qd - CO.vel(N), uv) for uv in N])
qdots = solve(kindiffs, qd)
# Set angular velocity of remaining frames
B.set_ang_vel(N, w_b_n_qd.subs(qdots))
C.set_ang_acc(N, C.ang_vel_in(N).dt(B) + cross(B.ang_vel_in(N), C.ang_vel_in(N)))
# Active forces
F_CO = m*g*A.z
# Create inertia dyadic of disc C about point CO
I = (m * r**2) / 4
J = (m * r**2) / 2
I_C_CO = inertia(C, I, J, I)
Disc = RigidBody('Disc', CO, C, m, (I_C_CO, CO))
BL = [Disc]
FL = [(CO, F_CO)]
KM = KanesMethod(N, [q1, q2, q3, q4, q5], [u1, u2, u3], kd_eqs=kindiffs,
q_dependent=[q6], configuration_constraints=f_c,
u_dependent=[u4, u5, u6], velocity_constraints=f_v)
with warnings.catch_warnings():
warnings.filterwarnings("ignore", category=SymPyDeprecationWarning)
(fr, fr_star) = KM.kanes_equations(FL, BL)
# Test generalized form equations
linearizer = KM.to_linearizer()
assert linearizer.f_c == f_c
assert linearizer.f_v == f_v
assert linearizer.f_a == f_v.diff(t)
sol = solve(linearizer.f_0 + linearizer.f_1, qd)
for qi in qd:
assert sol[qi] == qdots[qi]
assert simplify(linearizer.f_2 + linearizer.f_3 - fr - fr_star) == Matrix([0, 0, 0])
# Perform the linearization
# Precomputed operating point
q_op = {q6: -r*cos(q2)}
u_op = {u1: 0,
u2: sin(q2)*q1d + q3d,
u3: cos(q2)*q1d,
u4: -r*(sin(q2)*q1d + q3d)*cos(q3),
u5: 0,
u6: -r*(sin(q2)*q1d + q3d)*sin(q3)}
qd_op = {q2d: 0,
q4d: -r*(sin(q2)*q1d + q3d)*cos(q1),
q5d: -r*(sin(q2)*q1d + q3d)*sin(q1),
q6d: 0}
ud_op = {u1d: 4*g*sin(q2)/(5*r) + sin(2*q2)*q1d**2/2 + 6*cos(q2)*q1d*q3d/5,
u2d: 0,
u3d: 0,
u4d: r*(sin(q2)*sin(q3)*q1d*q3d + sin(q3)*q3d**2),
u5d: r*(4*g*sin(q2)/(5*r) + sin(2*q2)*q1d**2/2 + 6*cos(q2)*q1d*q3d/5),
u6d: -r*(sin(q2)*cos(q3)*q1d*q3d + cos(q3)*q3d**2)}
A, B = linearizer.linearize(op_point=[q_op, u_op, qd_op, ud_op], A_and_B=True, simplify=True)
upright_nominal = {q1d: 0, q2: 0, m: 1, r: 1, g: 1}
# Precomputed solution
A_sol = Matrix([[0, 0, 0, 0, 0, 0, 0, 1],
[0, 0, 0, 0, 0, 1, 0, 0],
[0, 0, 0, 0, 0, 0, 1, 0],
[sin(q1)*q3d, 0, 0, 0, 0, -sin(q1), -cos(q1), 0],
[-cos(q1)*q3d, 0, 0, 0, 0, cos(q1), -sin(q1), 0],
[0, 4/5, 0, 0, 0, 0, 0, 6*q3d/5],
[0, 0, 0, 0, 0, 0, 0, 0],
[0, 0, 0, 0, 0, -2*q3d, 0, 0]])
B_sol = Matrix([])
# Check that linearization is correct
assert A.subs(upright_nominal) == A_sol
assert B.subs(upright_nominal) == B_sol
# Check eigenvalues at critical speed are all zero:
assert A.subs(upright_nominal).subs(q3d, 1/sqrt(3)).eigenvals() == {0: 8}
class KanesMethod(object):
"""Kane's method object.
This object is used to do the "book-keeping" as you go through and form
equations of motion in the way Kane presents in:
Kane, T., Levinson, D. Dynamics Theory and Applications. 1985 McGraw-Hill
The attributes are for equations in the form [M] udot = forcing.
Attributes
==========
q, u : Matrix
Matrices of the generalized coordinates and speeds
bodylist : iterable
Iterable of Point and RigidBody objects in the system.
forcelist : iterable
Iterable of (Point, vector) or (ReferenceFrame, vector) tuples
describing the forces on the system.
auxiliary : Matrix
If applicable, the set of auxiliary Kane's
equations used to solve for non-contributing
forces.
mass_matrix : Matrix
The system's mass matrix
forcing : Matrix
The system's forcing vector
mass_matrix_full : Matrix
The "mass matrix" for the u's and q's
forcing_full : Matrix
The "forcing vector" for the u's and q's
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 speeds and coordinates and their
derivatives.
Then we create a point and set its velocity in a frame.
>>> from sympy import symbols
>>> from sympy.physics.mechanics import dynamicsymbols, ReferenceFrame
>>> from sympy.physics.mechanics import Point, Particle, KanesMethod
>>> q, u = dynamicsymbols('q u')
>>> qd, ud = dynamicsymbols('q u', 1)
>>> m, c, k = symbols('m c k')
>>> N = ReferenceFrame('N')
>>> P = Point('P')
>>> P.set_vel(N, u * N.x)
Next we need to arrange/store information in the way that KanesMethod
requires. The kinematic differential equations need to be stored in a
dict. A list of forces/torques must be constructed, where each entry in
the list is a (Point, Vector) or (ReferenceFrame, Vector) tuple, where the
Vectors represent the Force or Torque.
Next a particle needs to be created, and it needs to have a point and mass
assigned to it.
Finally, a list of all bodies and particles needs to be created.
>>> kd = [qd - u]
>>> FL = [(P, (-k * q - c * u) * N.x)]
>>> pa = Particle('pa', P, m)
>>> BL = [pa]
Finally we can generate the equations of motion.
First we create the KanesMethod object and supply an inertial frame,
coordinates, generalized speeds, and the kinematic differential equations.
Additional quantities such as configuration and motion constraints,
dependent coordinates and speeds, and auxiliary speeds are also supplied
here (see the online documentation).
Next we form FR* and FR to complete: Fr + Fr* = 0.
We have the equations of motion at this point.
It makes sense to rearrnge them though, so we calculate the mass matrix and
the forcing terms, for E.o.M. in the form: [MM] udot = forcing, where MM is
the mass matrix, udot is a vector of the time derivatives of the
generalized speeds, and forcing is a vector representing "forcing" terms.
>>> KM = KanesMethod(N, q_ind=[q], u_ind=[u], kd_eqs=kd)
>>> (fr, frstar) = KM.kanes_equations(BL, FL)
>>> MM = KM.mass_matrix
>>> forcing = KM.forcing
>>> rhs = MM.inv() * forcing
>>> rhs
Matrix([[(-c*u(t) - k*q(t))/m]])
>>> KM.linearize(A_and_B=True, new_method=True)[0]
Matrix([
[ 0, 1],
[-k/m, -c/m]])
Please look at the documentation pages for more information on how to
perform linearization and how to deal with dependent coordinates & speeds,
and how do deal with bringing non-contributing forces into evidence.
"""
def __init__(self, frame, q_ind, u_ind, kd_eqs=None, q_dependent=None,
configuration_constraints=None, u_dependent=None,
velocity_constraints=None, acceleration_constraints=None,
u_auxiliary=None):
"""Please read the online documentation. """
if not isinstance(frame, ReferenceFrame):
raise TypeError('An intertial ReferenceFrame must be supplied')
self._inertial = frame
self._fr = None
self._frstar = None
self._forcelist = None
self._bodylist = None
self._initialize_vectors(q_ind, q_dependent, u_ind, u_dependent,
u_auxiliary)
self._initialize_kindiffeq_matrices(kd_eqs)
self._initialize_constraint_matrices(configuration_constraints,
velocity_constraints, acceleration_constraints)
def _initialize_vectors(self, q_ind, q_dep, u_ind, u_dep, u_aux):
"""Initialize the coordinate and speed vectors."""
none_handler = lambda x: Matrix(x) if x else Matrix()
# Initialize generalized coordinates
q_dep = none_handler(q_dep)
if not iterable(q_ind):
raise TypeError('Generalized coordinates must be an iterable.')
if not iterable(q_dep):
raise TypeError('Dependent coordinates must be an iterable.')
q_ind = Matrix(q_ind)
self._qdep = q_dep
self._q = Matrix([q_ind, q_dep])
self._qdot = self.q.diff(dynamicsymbols._t)
# Initialize generalized speeds
u_dep = none_handler(u_dep)
if not iterable(u_ind):
raise TypeError('Generalized speeds must be an iterable.')
if not iterable(u_dep):
raise TypeError('Dependent speeds must be an iterable.')
u_ind = Matrix(u_ind)
self._udep = u_dep
self._u = Matrix([u_ind, u_dep])
self._udot = self.u.diff(dynamicsymbols._t)
self._uaux = none_handler(u_aux)
def _initialize_constraint_matrices(self, config, vel, acc):
"""Initializes constraint matrices."""
# Define vector dimensions
o = len(self.u)
m = len(self._udep)
p = o - m
none_handler = lambda x: Matrix(x) if x else Matrix()
# Initialize configuration constraints
config = none_handler(config)
if len(self._qdep) != len(config):
raise ValueError('There must be an equal number of dependent '
'coordinates and configuration constraints.')
self._f_h = none_handler(config)
# Initialize velocity and acceleration constraints
vel = none_handler(vel)
acc = none_handler(acc)
if len(vel) != m:
raise ValueError('There must be an equal number of dependent '
'speeds and velocity constraints.')
if acc and (len(acc) != m):
raise ValueError('There must be an equal number of dependent '
'speeds and acceleration constraints.')
if vel:
u_zero = dict((i, 0) for i in self.u)
udot_zero = dict((i, 0) for i in self._udot)
# When calling kanes_equations, another class instance will be
# created if auxiliary u's are present. In this case, the
# computation of kinetic differential equation matrices will be
# skipped as this was computed during the original KanesMethod
# object, and the qd_u_map will not be available.
if self._qdot_u_map is not None:
vel = msubs(vel, self._qdot_u_map)
self._f_nh = msubs(vel, u_zero)
self._k_nh = (vel - self._f_nh).jacobian(self.u)
# If no acceleration constraints given, calculate them.
if not acc:
self._f_dnh = (self._k_nh.diff(dynamicsymbols._t) * self.u +
self._f_nh.diff(dynamicsymbols._t))
self._k_dnh = self._k_nh
else:
if self._qdot_u_map is not None:
acc = msubs(acc, self._qdot_u_map)
self._f_dnh = msubs(acc, udot_zero)
self._k_dnh = (acc - self._f_dnh).jacobian(self._udot)
# Form of non-holonomic constraints is B*u + C = 0.
# We partition B into independent and dependent columns:
# Ars is then -B_dep.inv() * B_ind, and it relates dependent speeds
# to independent speeds as: udep = Ars*uind, neglecting the C term.
B_ind = self._k_nh[:, :p]
B_dep = self._k_nh[:, p:o]
self._Ars = -B_dep.LUsolve(B_ind)
else:
self._f_nh = Matrix()
self._k_nh = Matrix()
self._f_dnh = Matrix()
self._k_dnh = Matrix()
self._Ars = Matrix()
def _initialize_kindiffeq_matrices(self, kdeqs):
"""Initialize the kinematic differential equation matrices."""
if kdeqs:
if len(self.q) != len(kdeqs):
raise ValueError('There must be an equal number of kinematic '
'differential equations and coordinates.')
kdeqs = Matrix(kdeqs)
u = self.u
qdot = self._qdot
# Dictionaries setting things to zero
u_zero = dict((i, 0) for i in u)
uaux_zero = dict((i, 0) for i in self._uaux)
qdot_zero = dict((i, 0) for i in qdot)
f_k = msubs(kdeqs, u_zero, qdot_zero)
k_ku = (msubs(kdeqs, qdot_zero) - f_k).jacobian(u)
k_kqdot = (msubs(kdeqs, u_zero) - f_k).jacobian(qdot)
f_k = k_kqdot.LUsolve(f_k)
k_ku = k_kqdot.LUsolve(k_ku)
k_kqdot = eye(len(qdot))
self._qdot_u_map = solve_linear_system_LU(
Matrix([k_kqdot.T, -(k_ku * u + f_k).T]).T, qdot)
self._f_k = msubs(f_k, uaux_zero)
self._k_ku = msubs(k_ku, 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()
def _form_fr(self, fl):
"""Form the generalized active force."""
if fl != None and (len(fl) == 0 or not iterable(fl)):
raise ValueError('Force pairs must be supplied in an '
'non-empty iterable or None.')
N = self._inertial
# pull out relevant velocities for constructing partial velocities
vel_list, f_list = _f_list_parser(fl, N)
vel_list = [msubs(i, self._qdot_u_map) for i in vel_list]
# Fill Fr with dot product of partial velocities and forces
o = len(self.u)
b = len(f_list)
FR = zeros(o, 1)
partials = partial_velocity(vel_list, self.u, N)
for i in range(o):
FR[i] = sum(partials[j][i] & f_list[j] for j in range(b))
# In case there are dependent speeds
if self._udep:
p = o - len(self._udep)
FRtilde = FR[:p, 0]
FRold = FR[p:o, 0]
FRtilde += self._Ars.T * FRold
FR = FRtilde
self._forcelist = fl
self._fr = FR
return FR
def _form_frstar(self, bl):
"""Form the generalized inertia force."""
if not iterable(bl):
raise TypeError('Bodies must be supplied in an iterable.')
t = dynamicsymbols._t
N = self._inertial
# Dicts setting things to zero
udot_zero = dict((i, 0) for i in self._udot)
uaux_zero = dict((i, 0) for i in self._uaux)
uauxdot = [diff(i, t) for i in self._uaux]
uauxdot_zero = dict((i, 0) for i in uauxdot)
# Dictionary of q' and q'' to u and u'
q_ddot_u_map = dict((k.diff(t), v.diff(t)) for (k, v) in
self._qdot_u_map.items())
q_ddot_u_map.update(self._qdot_u_map)
# Fill up the list of partials: format is a list with num elements
# equal to number of entries in body list. Each of these elements is a
# list - either of length 1 for the translational components of
# particles or of length 2 for the translational and rotational
# components of rigid bodies. The inner most list is the list of
# partial velocities.
def get_partial_velocity(body):
if isinstance(body, RigidBody):
vlist = [body.masscenter.vel(N), body.frame.ang_vel_in(N)]
elif isinstance(body, Particle):
vlist = [body.point.vel(N),]
else:
raise TypeError('The body list may only contain either '
'RigidBody or Particle as list elements.')
v = [msubs(vel, self._qdot_u_map) for vel in vlist]
return partial_velocity(v, self.u, N)
partials = [get_partial_velocity(body) for body in bl]
# Compute fr_star in two components:
# fr_star = -(MM*u' + nonMM)
o = len(self.u)
MM = zeros(o, o)
nonMM = zeros(o, 1)
zero_uaux = lambda expr: msubs(expr, uaux_zero)
zero_udot_uaux = lambda expr: msubs(msubs(expr, udot_zero), uaux_zero)
for i, body in enumerate(bl):
if isinstance(body, RigidBody):
M = zero_uaux(body.mass)
I = zero_uaux(body.central_inertia)
vel = zero_uaux(body.masscenter.vel(N))
omega = zero_uaux(body.frame.ang_vel_in(N))
acc = zero_udot_uaux(body.masscenter.acc(N))
inertial_force = (M.diff(t) * vel + M * acc)
inertial_torque = zero_uaux((I.dt(body.frame) & omega) +
msubs(I & body.frame.ang_acc_in(N), udot_zero) +
(omega ^ (I & omega)))
for j in range(o):
tmp_vel = zero_uaux(partials[i][0][j])
tmp_ang = zero_uaux(I & partials[i][1][j])
for k in range(o):
# translational
MM[j, k] += M * (tmp_vel & partials[i][0][k])
# rotational
MM[j, k] += (tmp_ang & partials[i][1][k])
nonMM[j] += inertial_force & partials[i][0][j]
nonMM[j] += inertial_torque & partials[i][1][j]
else:
M = zero_uaux(body.mass)
vel = zero_uaux(body.point.vel(N))
acc = zero_udot_uaux(body.point.acc(N))
inertial_force = (M.diff(t) * vel + M * acc)
for j in range(o):
temp = zero_uaux(partials[i][0][j])
for k in range(o):
MM[j, k] += M * (temp & partials[i][0][k])
nonMM[j] += inertial_force & partials[i][0][j]
# Compose fr_star out of MM and nonMM
MM = zero_uaux(msubs(MM, q_ddot_u_map))
nonMM = msubs(msubs(nonMM, q_ddot_u_map),
udot_zero, uauxdot_zero, uaux_zero)
fr_star = -(MM * msubs(Matrix(self._udot), uauxdot_zero) + nonMM)
# If there are dependent speeds, we need to find fr_star_tilde
if self._udep:
p = o - len(self._udep)
fr_star_ind = fr_star[:p, 0]
fr_star_dep = fr_star[p:o, 0]
fr_star = fr_star_ind + (self._Ars.T * fr_star_dep)
# Apply the same to MM
MMi = MM[:p, :]
MMd = MM[p:o, :]
MM = MMi + (self._Ars.T * MMd)
self._bodylist = bl
self._frstar = fr_star
self._k_d = MM
self._f_d = -msubs(self._fr + self._frstar, udot_zero)
return fr_star
def to_linearizer(self):
"""Returns an instance of the Linearizer class, initiated from the
data in the KanesMethod 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)."""
if (self._fr is None) or (self._frstar is None):
raise ValueError('Need to compute Fr, Fr* first.')
# Get required equation components. The Kane's method class breaks
# these into pieces. Need to reassemble
f_c = self._f_h
if self._f_nh and self._k_nh:
f_v = self._f_nh + self._k_nh*Matrix(self.u)
else:
f_v = Matrix()
if self._f_dnh and self._k_dnh:
f_a = self._f_dnh + self._k_dnh*Matrix(self._udot)
else:
f_a = Matrix()
# Dicts to sub to zero, for splitting up expressions
u_zero = dict((i, 0) for i in self.u)
ud_zero = dict((i, 0) for i in self._udot)
qd_zero = dict((i, 0) for i in self._qdot)
qd_u_zero = dict((i, 0) for i in Matrix([self._qdot, self.u]))
# Break the kinematic differential eqs apart into f_0 and f_1
f_0 = msubs(self._f_k, u_zero) + self._k_kqdot*Matrix(self._qdot)
f_1 = msubs(self._f_k, qd_zero) + self._k_ku*Matrix(self.u)
# Break the dynamic differential eqs into f_2 and f_3
f_2 = msubs(self._frstar, qd_u_zero)
f_3 = msubs(self._frstar, ud_zero) + self._fr
f_4 = zeros(len(f_2), 1)
# Get the required vector components
q = self.q
u = self.u
if self._qdep:
q_i = q[:-len(self._qdep)]
else:
q_i = q
q_d = self._qdep
if self._udep:
u_i = u[:-len(self._udep)]
else:
u_i = u
u_d = self._udep
# Form dictionary to set auxiliary speeds & their derivatives to 0.
uaux = self._uaux
uauxdot = uaux.diff(dynamicsymbols._t)
uaux_zero = dict((i, 0) for i in Matrix([uaux, uauxdot]))
# Checking for dynamic symbols outside the dynamic differential
# equations; throws error if there is.
sym_list = set(Matrix([q, self._qdot, u, self._udot, uaux, uauxdot]))
if any(find_dynamicsymbols(i, sym_list) for i in [self._k_kqdot,
self._k_ku, self._f_k, self._k_dnh, self._f_dnh, self._k_d]):
raise ValueError('Cannot have dynamicsymbols outside dynamic \
forcing vector.')
# Find all other dynamic symbols, forming the forcing vector r.
# Sort r to make it canonical.
r = list(find_dynamicsymbols(msubs(self._f_d, uaux_zero), sym_list))
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)
def linearize(self, **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.
As part of the deprecation cycle, the new method will not be used unless
the kwarg ``new_method`` is set to True. If the kwarg is missing, or set
to false, the old linearization method will be used. After next release
the need for this kwarg will be removed.
For more documentation, please see the ``Linearizer`` class."""
if 'new_method' not in kwargs or not kwargs['new_method']:
# User is still using old code.
SymPyDeprecationWarning('The linearize class method has changed '
'to a new interface, the old method is deprecated. To '
'use the new method, set the kwarg `new_method=True`. '
'For more information, read the docstring '
'of `linearize`.').warn()
return self._old_linearize()
# Remove the new method flag, before passing kwargs to linearize
kwargs.pop('new_method')
linearizer = self.to_linearizer()
result = linearizer.linearize(**kwargs)
return result + (linearizer.r,)
def _old_linearize(self):
"""Old method to linearize the equations of motion. Returns a tuple of
(f_lin_A, f_lin_B, y) for forming [M]qudot = [f_lin_A]qu + [f_lin_B]y.
Deprecated in favor of new method using Linearizer class. Please change
your code to use the new `linearize` method."""
if (self._fr is None) or (self._frstar is None):
raise ValueError('Need to compute Fr, Fr* first.')
# Note that this is now unneccessary, and it should never be
# encountered; I still think it should be in here in case the user
# manually sets these matrices incorrectly.
for i in self.q:
if self._k_kqdot.diff(i) != 0 * self._k_kqdot:
raise ValueError('Matrix K_kqdot must not depend on any q.')
t = dynamicsymbols._t
uaux = self._uaux
uauxdot = [diff(i, t) for i in uaux]
# dictionary of auxiliary speeds & derivatives which are equal to zero
subdict = dict(zip(uaux[:] + uauxdot[:],
[0] * (len(uaux) + len(uauxdot))))
# Checking for dynamic symbols outside the dynamic differential
# equations; throws error if there is.
insyms = set(self.q[:] + self._qdot[:] + self.u[:] + self._udot[:] +
uaux[:] + uauxdot)
if any(find_dynamicsymbols(i, insyms) for i in [self._k_kqdot,
self._k_ku, self._f_k, self._k_dnh, self._f_dnh, self._k_d]):
raise ValueError('Cannot have dynamicsymbols outside dynamic \
forcing vector.')
other_dyns = list(find_dynamicsymbols(msubs(self._f_d, subdict), insyms))
# make it canonically ordered so the jacobian is canonical
other_dyns.sort(key=default_sort_key)
for i in other_dyns:
if diff(i, dynamicsymbols._t) in other_dyns:
raise ValueError('Cannot have derivatives of specified '
'quantities when linearizing forcing terms.')
o = len(self.u) # number of speeds
n = len(self.q) # number of coordinates
l = len(self._qdep) # number of configuration constraints
m = len(self._udep) # number of motion constraints
qi = Matrix(self.q[: n - l]) # independent coords
qd = Matrix(self.q[n - l: n]) # dependent coords; could be empty
ui = Matrix(self.u[: o - m]) # independent speeds
ud = Matrix(self.u[o - m: o]) # dependent speeds; could be empty
qdot = Matrix(self._qdot) # time derivatives of coordinates
# with equations in the form MM udot = forcing, expand that to:
# MM_full [q,u].T = forcing_full. This combines coordinates and
# speeds together for the linearization, which is necessary for the
# linearization process, due to dependent coordinates. f1 is the rows
# from the kinematic differential equations, f2 is the rows from the
# dynamic differential equations (and differentiated non-holonomic
# constraints).
f1 = self._k_ku * Matrix(self.u) + self._f_k
f2 = self._f_d
# Only want to do this if these matrices have been filled in, which
# occurs when there are dependent speeds
if m != 0:
f2 = self._f_d.col_join(self._f_dnh)
fnh = self._f_nh + self._k_nh * Matrix(self.u)
f1 = msubs(f1, subdict)
f2 = msubs(f2, subdict)
fh = msubs(self._f_h, subdict)
fku = msubs(self._k_ku * Matrix(self.u), subdict)
fkf = msubs(self._f_k, subdict)
# In the code below, we are applying the chain rule by hand on these
# things. All the matrices have been changed into vectors (by
# multiplying the dynamic symbols which it is paired with), so we can
# take the jacobian of them. The basic operation is take the jacobian
# of the f1, f2 vectors wrt all of the q's and u's. f1 is a function of
# q, u, and t; f2 is a function of q, qdot, u, and t. In the code
# below, we are not considering perturbations in t. So if f1 is a
# function of the q's, u's but some of the q's or u's could be
# dependent on other q's or u's (qd's might be dependent on qi's, ud's
# might be dependent on ui's or qi's), so what we do is take the
# jacobian of the f1 term wrt qi's and qd's, the jacobian wrt the qd's
# gets multiplied by the jacobian of qd wrt qi, this is extended for
# the ud's as well. dqd_dqi is computed by taking a taylor expansion of
# the holonomic constraint equations about q*, treating q* - q as dq,
# separating into dqd (depedent q's) and dqi (independent q's) and the
# rearranging for dqd/dqi. This is again extended for the speeds.
# First case: configuration and motion constraints
if (l != 0) and (m != 0):
fh_jac_qi = fh.jacobian(qi)
fh_jac_qd = fh.jacobian(qd)
fnh_jac_qi = fnh.jacobian(qi)
fnh_jac_qd = fnh.jacobian(qd)
fnh_jac_ui = fnh.jacobian(ui)
fnh_jac_ud = fnh.jacobian(ud)
fku_jac_qi = fku.jacobian(qi)
fku_jac_qd = fku.jacobian(qd)
fku_jac_ui = fku.jacobian(ui)
fku_jac_ud = fku.jacobian(ud)
fkf_jac_qi = fkf.jacobian(qi)
fkf_jac_qd = fkf.jacobian(qd)
f1_jac_qi = f1.jacobian(qi)
f1_jac_qd = f1.jacobian(qd)
f1_jac_ui = f1.jacobian(ui)
f1_jac_ud = f1.jacobian(ud)
f2_jac_qi = f2.jacobian(qi)
f2_jac_qd = f2.jacobian(qd)
f2_jac_ui = f2.jacobian(ui)
f2_jac_ud = f2.jacobian(ud)
f2_jac_qdot = f2.jacobian(qdot)
dqd_dqi = - fh_jac_qd.LUsolve(fh_jac_qi)
dud_dqi = fnh_jac_ud.LUsolve(fnh_jac_qd * dqd_dqi - fnh_jac_qi)
dud_dui = - fnh_jac_ud.LUsolve(fnh_jac_ui)
dqdot_dui = - self._k_kqdot.inv() * (fku_jac_ui +
fku_jac_ud * dud_dui)
dqdot_dqi = - self._k_kqdot.inv() * (fku_jac_qi + fkf_jac_qi +
(fku_jac_qd + fkf_jac_qd) * dqd_dqi + fku_jac_ud * dud_dqi)
f1_q = f1_jac_qi + f1_jac_qd * dqd_dqi + f1_jac_ud * dud_dqi
f1_u = f1_jac_ui + f1_jac_ud * dud_dui
f2_q = (f2_jac_qi + f2_jac_qd * dqd_dqi + f2_jac_qdot * dqdot_dqi +
f2_jac_ud * dud_dqi)
f2_u = f2_jac_ui + f2_jac_ud * dud_dui + f2_jac_qdot * dqdot_dui
# Second case: configuration constraints only
elif l != 0:
dqd_dqi = - fh.jacobian(qd).LUsolve(fh.jacobian(qi))
dqdot_dui = - self._k_kqdot.inv() * fku.jacobian(ui)
dqdot_dqi = - self._k_kqdot.inv() * (fku.jacobian(qi) +
fkf.jacobian(qi) + (fku.jacobian(qd) + fkf.jacobian(qd)) *
dqd_dqi)
f1_q = (f1.jacobian(qi) + f1.jacobian(qd) * dqd_dqi)
f1_u = f1.jacobian(ui)
f2_jac_qdot = f2.jacobian(qdot)
f2_q = (f2.jacobian(qi) + f2.jacobian(qd) * dqd_dqi +
f2.jac_qdot * dqdot_dqi)
f2_u = f2.jacobian(ui) + f2_jac_qdot * dqdot_dui
# Third case: motion constraints only
elif m != 0:
dud_dqi = fnh.jacobian(ud).LUsolve(- fnh.jacobian(qi))
dud_dui = - fnh.jacobian(ud).LUsolve(fnh.jacobian(ui))
dqdot_dui = - self._k_kqdot.inv() * (fku.jacobian(ui) +
fku.jacobian(ud) * dud_dui)
dqdot_dqi = - self._k_kqdot.inv() * (fku.jacobian(qi) +
fkf.jacobian(qi) + fku.jacobian(ud) * dud_dqi)
f1_jac_ud = f1.jacobian(ud)
f2_jac_qdot = f2.jacobian(qdot)
f2_jac_ud = f2.jacobian(ud)
f1_q = f1.jacobian(qi) + f1_jac_ud * dud_dqi
f1_u = f1.jacobian(ui) + f1_jac_ud * dud_dui
f2_q = (f2.jacobian(qi) + f2_jac_qdot * dqdot_dqi + f2_jac_ud
* dud_dqi)
f2_u = (f2.jacobian(ui) + f2_jac_ud * dud_dui + f2_jac_qdot *
dqdot_dui)
# Fourth case: No constraints
else:
dqdot_dui = - self._k_kqdot.inv() * fku.jacobian(ui)
dqdot_dqi = - self._k_kqdot.inv() * (fku.jacobian(qi) +
fkf.jacobian(qi))
f1_q = f1.jacobian(qi)
f1_u = f1.jacobian(ui)
f2_jac_qdot = f2.jacobian(qdot)
f2_q = f2.jacobian(qi) + f2_jac_qdot * dqdot_dqi
f2_u = f2.jacobian(ui) + f2_jac_qdot * dqdot_dui
f_lin_A = -(f1_q.row_join(f1_u)).col_join(f2_q.row_join(f2_u))
if other_dyns:
f1_oths = f1.jacobian(other_dyns)
f2_oths = f2.jacobian(other_dyns)
f_lin_B = -f1_oths.col_join(f2_oths)
else:
f_lin_B = Matrix()
return (f_lin_A, f_lin_B, Matrix(other_dyns))
def kanes_equations(self, bodies, loads=None):
""" Method to form Kane's equations, Fr + Fr* = 0.
Returns (Fr, Fr*). In the case where auxiliary generalized speeds are
present (say, s auxiliary speeds, o generalized speeds, and m motion
constraints) the length of the returned vectors will be o - m + s in
length. The first o - m equations will be the constrained Kane's
equations, then the s auxiliary Kane's equations. These auxiliary
equations can be accessed with the auxiliary_eqs().
Parameters
==========
bodies : iterable
An iterable of all RigidBody's and Particle's in the system.
A system must have at least one body.
loads : iterable
Takes in an iterable of (Particle, Vector) or (ReferenceFrame, Vector)
tuples which represent the force at a point or torque on a frame.
Must be either a non-empty iterable of tuples or None which corresponds
to a system with no constraints.
"""
if (bodies is None and loads != None) or isinstance(bodies[0], tuple):
# This switches the order if they use the old way.
bodies, loads = loads, bodies
SymPyDeprecationWarning(value='The API for kanes_equations() has changed such '
'that the loads (forces and torques) are now the second argument '
'and is optional with None being the default.',
feature='The kanes_equation() argument order',
useinstead='switched argument order to update your code, For example: '
'kanes_equations(loads, bodies) > kanes_equations(bodies, loads).',
issue=10945, deprecated_since_version="1.1").warn()
if not self._k_kqdot:
raise AttributeError('Create an instance of KanesMethod with '
'kinematic differential equations to use this method.')
fr = self._form_fr(loads)
frstar = self._form_frstar(bodies)
if self._uaux:
if not self._udep:
km = KanesMethod(self._inertial, self.q, self._uaux,
u_auxiliary=self._uaux)
else:
km = KanesMethod(self._inertial, self.q, self._uaux,
u_auxiliary=self._uaux, u_dependent=self._udep,
velocity_constraints=(self._k_nh * self.u +
self._f_nh))
km._qdot_u_map = self._qdot_u_map
self._km = km
fraux = km._form_fr(loads)
frstaraux = km._form_frstar(bodies)
self._aux_eq = fraux + frstaraux
self._fr = fr.col_join(fraux)
self._frstar = frstar.col_join(frstaraux)
return (self._fr, self._frstar)
def rhs(self, inv_method=None):
"""Returns the system's equations of motion in first order form. The
output is the right hand side of::
x' = |q'| =: f(q, u, r, p, t)
|u'|
The right hand side is what is needed by most numerical ODE
integrators.
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`
"""
rhs = zeros(len(self.q) + len(self.u), c=1)
kdes = self.kindiffdict()
for i, q_i in enumerate(self.q):
rhs[i] = kdes[q_i.diff()]
if inv_method is None:
rhs[len(self.q):, 0] = self.mass_matrix.LUsolve(self.forcing)
else:
rhs[len(self.q):, 0] = (self.mass_matrix.inv(inv_method,
try_block_diag=True) *
self.forcing)
return rhs
def kindiffdict(self):
"""Returns a dictionary mapping q' to u."""
if not self._qdot_u_map:
raise AttributeError('Create an instance of KanesMethod with '
'kinematic differential equations to use this method.')
return self._qdot_u_map
@property
def auxiliary_eqs(self):
"""A matrix containing the auxiliary equations."""
if not self._fr or not self._frstar:
raise ValueError('Need to compute Fr, Fr* first.')
if not self._uaux:
raise ValueError('No auxiliary speeds have been declared.')
return self._aux_eq
@property
def mass_matrix(self):
"""The mass matrix of the system."""
if not self._fr or not self._frstar:
raise ValueError('Need to compute Fr, Fr* first.')
return Matrix([self._k_d, self._k_dnh])
@property
def mass_matrix_full(self):
"""The mass matrix of the system, augmented by the kinematic
differential equations."""
if not self._fr or not self._frstar:
raise ValueError('Need to compute Fr, Fr* first.')
o = len(self.u)
n = len(self.q)
return ((self._k_kqdot).row_join(zeros(n, o))).col_join((zeros(o,
n)).row_join(self.mass_matrix))
@property
def forcing(self):
"""The forcing vector of the system."""
if not self._fr or not self._frstar:
raise ValueError('Need to compute Fr, Fr* first.')
return -Matrix([self._f_d, self._f_dnh])
@property
def forcing_full(self):
"""The forcing vector of the system, augmented by the kinematic
differential equations."""
if not self._fr or not self._frstar:
raise ValueError('Need to compute Fr, Fr* first.')
f1 = self._k_ku * Matrix(self.u) + self._f_k
return -Matrix([f1, self._f_d, self._f_dnh])
@property
def q(self):
return self._q
@property
def u(self):
return self._u
@property
def bodylist(self):
return self._bodylist
@property
def forcelist(self):
return self._forcelist
