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 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])
def test_form_2():
symsystem2 = SymbolicSystem(coordinates,
comb_implicit_rhs,
speeds=speeds,
mass_matrix=comb_implicit_mat,
alg_con=alg_con_full,
output_eqns=out_eqns,
bodies=bodies,
loads=loads)
assert symsystem2.coordinates == Matrix([x, y, lam])
assert symsystem2.speeds == Matrix([u, v])
assert symsystem2.states == Matrix([x, y, lam, u, v])
assert symsystem2.alg_con == [4]
inter = comb_implicit_rhs
assert simplify(symsystem2.comb_implicit_rhs - inter) == zeros(5, 1)
assert simplify(symsystem2.comb_implicit_mat -
comb_implicit_mat) == zeros(5)
assert set(symsystem2.dynamic_symbols()) == {y, v, lam, u, x}
assert type(symsystem2.dynamic_symbols()) == tuple
assert set(symsystem2.constant_symbols()) == {l, g, m}
assert type(symsystem2.constant_symbols()) == tuple
inter = comb_explicit_rhs
symsystem2.compute_explicit_form()
assert simplify(symsystem2.comb_explicit_rhs - inter) == zeros(5, 1)
assert symsystem2.output_eqns == out_eqns
assert symsystem2.bodies == (Pa, )
assert symsystem2.loads == ((P, g * m * N.x), )
def test_n_link_pendulum_on_cart_higher_order():
l0, m0 = symbols("l0 m0")
l1, m1 = symbols("l1 m1")
m2 = symbols("m2")
g = symbols("g")
q0, q1, q2 = dynamicsymbols("q0 q1 q2")
u0, u1, u2 = dynamicsymbols("u0 u1 u2")
F, T1 = dynamicsymbols("F T1")
kane1 = models.n_link_pendulum_on_cart(2)
massmatrix1 = Matrix([[m0 + m1 + m2, -l0*m1*cos(q1) - l0*m2*cos(q1),
-l1*m2*cos(q2)],
[-l0*m1*cos(q1) - l0*m2*cos(q1), l0**2*m1 + l0**2*m2,
l0*l1*m2*(sin(q1)*sin(q2) + cos(q1)*cos(q2))],
[-l1*m2*cos(q2),
l0*l1*m2*(sin(q1)*sin(q2) + cos(q1)*cos(q2)),
l1**2*m2]])
forcing1 = Matrix([[-l0*m1*u1**2*sin(q1) - l0*m2*u1**2*sin(q1) -
l1*m2*u2**2*sin(q2) + F],
[g*l0*m1*sin(q1) + g*l0*m2*sin(q1) -
l0*l1*m2*(sin(q1)*cos(q2) - sin(q2)*cos(q1))*u2**2],
[g*l1*m2*sin(q2) - l0*l1*m2*(-sin(q1)*cos(q2) +
sin(q2)*cos(q1))*u1**2]])
assert simplify(massmatrix1 - kane1.mass_matrix) == zeros(3)
assert simplify(forcing1 - kane1.forcing) == Matrix([0, 0, 0])
def test_linearize_pendulum_lagrange_nonminimal():
q1, q2 = dynamicsymbols("q1:3")
q1d, q2d = dynamicsymbols("q1: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])
# Create point P, the pendulum mass
P = pN.locatenew("P1", q1 * N.x + q2 * N.y)
P.set_vel(N, P.pos_from(pN).dt(N))
pP = Particle("pP", P, m)
# Constraint Equations
f_c = Matrix([q1 ** 2 + q2 ** 2 - L ** 2])
# Calculate the lagrangian, and form the equations of motion
Lag = Lagrangian(N, pP)
LM = LagrangesMethod(
Lag, [q1, q2], hol_coneqs=f_c, forcelist=[(P, m * g * N.x)], frame=N
)
LM.form_lagranges_equations()
# Compose operating point
op_point = {q1: L, q2: 0, q1d: 0, q2d: 0, q1d.diff(t): 0, q2d.diff(t): 0}
# Solve for multiplier operating point
lam_op = LM.solve_multipliers(op_point=op_point)
op_point.update(lam_op)
# Perform the Linearization
A, B, inp_vec = LM.linearize(
[q2], [q2d], [q1], [q1d], op_point=op_point, A_and_B=True
)
assert A == Matrix([[0, 1], [-9.8 / L, 0]])
assert B == Matrix([])
def _rot(axis, angle):
"""DCM for simple axis 1,2,or 3 rotations. """
if axis == 1:
return Matrix(
[
[1, 0, 0],
[0, cos(angle), -sin(angle)],
[0, sin(angle), cos(angle)],
]
)
elif axis == 2:
return Matrix(
[
[cos(angle), 0, sin(angle)],
[0, 1, 0],
[-sin(angle), 0, cos(angle)],
]
)
elif axis == 3:
return Matrix(
[
[cos(angle), -sin(angle), 0],
[sin(angle), cos(angle), 0],
[0, 0, 1],
]
)
def test_linearize_pendulum_lagrange_minimal():
q1 = dynamicsymbols("q1") # angle of pendulum
q1d = dynamicsymbols("q1", 1) # Angular velocity
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
A = N.orientnew("A", "axis", [q1, N.z])
A.set_ang_vel(N, q1d * N.z)
# Locate point P relative to the origin N*
P = pN.locatenew("P", L * A.x)
P.v2pt_theory(pN, N, A)
pP = Particle("pP", P, m)
# Solve for eom with Lagranges method
Lag = Lagrangian(N, pP)
LM = LagrangesMethod(Lag, [q1], forcelist=[(P, m * g * N.x)], frame=N)
LM.form_lagranges_equations()
# Linearize
A, B, inp_vec = LM.linearize([q1], [q1d], A_and_B=True)
assert A == Matrix([[0, 1], [-9.8 * cos(q1) / L, 0]])
assert B == Matrix([])
def test_form_1():
symsystem1 = SymbolicSystem(
states,
comb_explicit_rhs,
alg_con=alg_con_full,
output_eqns=out_eqns,
coord_idxs=coord_idxs,
speed_idxs=speed_idxs,
bodies=bodies,
loads=loads,
)
assert symsystem1.coordinates == Matrix([x, y])
assert symsystem1.speeds == Matrix([u, v])
assert symsystem1.states == Matrix([x, y, u, v, lam])
assert symsystem1.alg_con == [4]
inter = comb_explicit_rhs
assert simplify(symsystem1.comb_explicit_rhs - inter) == zeros(5, 1)
assert set(symsystem1.dynamic_symbols()) == set([y, v, lam, u, x])
assert type(symsystem1.dynamic_symbols()) == tuple
assert set(symsystem1.constant_symbols()) == set([l, g, m])
assert type(symsystem1.constant_symbols()) == tuple
assert symsystem1.output_eqns == out_eqns
assert symsystem1.bodies == (Pa, )
assert symsystem1.loads == ((P, g * m * N.x), )
def test_aux():
# Same as above, except we have 2 auxiliary speeds for the ground contact
# point, which is known to be zero. In one case, we go through then
# substitute the aux. speeds in at the end (they are zero, as well as their
# derivative), in the other case, we use the built-in auxiliary speed part
# of KanesMethod. The equations from each should be the same.
q1, q2, q3, u1, u2, u3 = dynamicsymbols('q1 q2 q3 u1 u2 u3')
q1d, q2d, q3d, u1d, u2d, u3d = dynamicsymbols('q1 q2 q3 u1 u2 u3', 1)
u4, u5, f1, f2 = dynamicsymbols('u4, u5, f1, f2')
u4d, u5d = dynamicsymbols('u4, u5', 1)
r, m, g = symbols('r m g')
N = ReferenceFrame('N')
Y = N.orientnew('Y', 'Axis', [q1, N.z])
L = Y.orientnew('L', 'Axis', [q2, Y.x])
R = L.orientnew('R', 'Axis', [q3, L.y])
w_R_N_qd = R.ang_vel_in(N)
R.set_ang_vel(N, u1 * L.x + u2 * L.y + u3 * L.z)
C = Point('C')
C.set_vel(N, u4 * L.x + u5 * (Y.z ^ L.x))
Dmc = C.locatenew('Dmc', r * L.z)
Dmc.v2pt_theory(C, N, R)
Dmc.a2pt_theory(C, N, R)
I = inertia(L, m / 4 * r**2, m / 2 * r**2, m / 4 * r**2)
kd = [dot(R.ang_vel_in(N) - w_R_N_qd, uv) for uv in L]
ForceList = [(Dmc, -m * g * Y.z), (C, f1 * L.x + f2 * (Y.z ^ L.x))]
BodyD = RigidBody('BodyD', Dmc, R, m, (I, Dmc))
BodyList = [BodyD]
KM = KanesMethod(N,
q_ind=[q1, q2, q3],
u_ind=[u1, u2, u3, u4, u5],
kd_eqs=kd)
with warnings.catch_warnings():
warnings.filterwarnings("ignore", category=SymPyDeprecationWarning)
(fr, frstar) = KM.kanes_equations(ForceList, BodyList)
fr = fr.subs({u4d: 0, u5d: 0}).subs({u4: 0, u5: 0})
frstar = frstar.subs({u4d: 0, u5d: 0}).subs({u4: 0, u5: 0})
KM2 = KanesMethod(N,
q_ind=[q1, q2, q3],
u_ind=[u1, u2, u3],
kd_eqs=kd,
u_auxiliary=[u4, u5])
with warnings.catch_warnings():
warnings.filterwarnings("ignore", category=SymPyDeprecationWarning)
(fr2, frstar2) = KM2.kanes_equations(ForceList, BodyList)
fr2 = fr2.subs({u4d: 0, u5d: 0}).subs({u4: 0, u5: 0})
frstar2 = frstar2.subs({u4d: 0, u5d: 0}).subs({u4: 0, u5: 0})
frstar.simplify()
frstar2.simplify()
assert (fr - fr2).expand() == Matrix([0, 0, 0, 0, 0])
assert (frstar - frstar2).expand() == Matrix([0, 0, 0, 0, 0])
def test_body_dcm():
A = Body('A')
B = Body('B')
A.frame.orient_axis(B.frame, B.frame.z, 10)
assert A.dcm(B) == Matrix([[cos(10), sin(10), 0], [-sin(10),
cos(10), 0], [0, 0, 1]])
assert A.dcm(B.frame) == Matrix([[cos(10), sin(10), 0],
[-sin(10), cos(10), 0], [0, 0, 1]])
def test_input_format():
# 1 dof problem from test_one_dof
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)
kd = [qd - u]
FL = [(P, (-k * q - c * u) * N.x)]
pa = Particle("pa", P, m)
BL = [pa]
KM = KanesMethod(N, [q], [u], kd)
# test for input format kane.kanes_equations((body1, body2, particle1))
assert KM.kanes_equations(BL)[0] == Matrix([0])
# test for input format kane.kanes_equations(bodies=(body1, body 2), loads=(load1,load2))
assert KM.kanes_equations(bodies=BL, loads=None)[0] == Matrix([0])
# test for input format kane.kanes_equations(bodies=(body1, body 2), loads=None)
assert KM.kanes_equations(BL, loads=None)[0] == Matrix([0])
# test for input format kane.kanes_equations(bodies=(body1, body 2))
assert KM.kanes_equations(BL)[0] == Matrix([0])
# test for error raised when a wrong force list (in this case a string) is provided
from sympy.testing.pytest import raises
raises(ValueError, lambda: KM._form_fr("bad input"))
# 2 dof problem from test_two_dof
q1, q2, u1, u2 = dynamicsymbols("q1 q2 u1 u2")
q1d, q2d, u1d, u2d = dynamicsymbols("q1 q2 u1 u2", 1)
m, c1, c2, k1, k2 = symbols("m c1 c2 k1 k2")
N = ReferenceFrame("N")
P1 = Point("P1")
P2 = Point("P2")
P1.set_vel(N, u1 * N.x)
P2.set_vel(N, (u1 + u2) * N.x)
kd = [q1d - u1, q2d - u2]
FL = (
(P1, (-k1 * q1 - c1 * u1 + k2 * q2 + c2 * u2) * N.x),
(P2, (-k2 * q2 - c2 * u2) * N.x),
)
pa1 = Particle("pa1", P1, m)
pa2 = Particle("pa2", P2, m)
BL = (pa1, pa2)
KM = KanesMethod(N, q_ind=[q1, q2], u_ind=[u1, u2], kd_eqs=kd)
# test for input format
# kane.kanes_equations((body1, body2), (load1, load2))
KM.kanes_equations(BL, FL)
MM = KM.mass_matrix
forcing = KM.forcing
rhs = MM.inv() * forcing
assert expand(rhs[0]) == expand(
(-k1 * q1 - c1 * u1 + k2 * q2 + c2 * u2) / m)
assert expand(rhs[1]) == expand(
(k1 * q1 + c1 * u1 - 2 * k2 * q2 - 2 * c2 * u2) / m)
def test_multi_mass_spring_damper_higher_order():
c0, k0, m0 = symbols("c0 k0 m0")
c1, k1, m1 = symbols("c1 k1 m1")
c2, k2, m2 = symbols("c2 k2 m2")
v0, x0 = dynamicsymbols("v0 x0")
v1, x1 = dynamicsymbols("v1 x1")
v2, x2 = dynamicsymbols("v2 x2")
kane1 = models.multi_mass_spring_damper(3)
massmatrix1 = Matrix([[m0 + m1 + m2, m1 + m2, m2], [m1 + m2, m1 + m2, m2],
[m2, m2, m2]])
forcing1 = Matrix([[-c0 * v0 - k0 * x0], [-c1 * v1 - k1 * x1],
[-c2 * v2 - k2 * x2]])
assert simplify(massmatrix1 - kane1.mass_matrix) == zeros(3)
assert simplify(forcing1 - kane1.forcing) == Matrix([0, 0, 0])
def test_one_dof():
# This is for a 1 dof spring-mass-damper case.
# It is described in more detail in the KanesMethod docstring.
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)
kd = [qd - u]
FL = [(P, (-k * q - c * u) * N.x)]
pa = Particle('pa', P, m)
BL = [pa]
KM = KanesMethod(N, [q], [u], kd)
KM.kanes_equations(BL, FL)
assert KM.bodies == BL
MM = KM.mass_matrix
forcing = KM.forcing
rhs = MM.inv() * forcing
assert expand(rhs[0]) == expand(-(q * k + u * c) / m)
assert simplify(KM.rhs() -
KM.mass_matrix_full.LUsolve(KM.forcing_full)) == zeros(
2, 1)
assert (KM.linearize(A_and_B=True, )[0] == Matrix([[0, 1],
[-k / m, -c / m]]))
def __init__(self, inlist):
"""This is the constructor for the Vector class. You shouldn't be
calling this, it should only be used by other functions. You should be
treating Vectors like you would with if you were doing the math by
hand, and getting the first 3 from the standard basis vectors from a
ReferenceFrame.
The only exception is to create a zero vector:
zv = Vector(0)
"""
self.args = []
if inlist == 0:
inlist = []
if isinstance(inlist, dict):
d = inlist
else:
d = {}
for inp in inlist:
if inp[1] in d:
d[inp[1]] += inp[0]
else:
d[inp[1]] = inp[0]
for k, v in d.items():
if v != Matrix([0, 0, 0]):
self.args.append((v, k))
def test_one_dof():
# This is for a 1 dof spring-mass-damper case.
# It is described in more detail in the KanesMethod docstring.
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)
kd = [qd - u]
FL = [(P, (-k * q - c * u) * N.x)]
pa = Particle('pa', P, m)
BL = [pa]
KM = KanesMethod(N, [q], [u], kd)
# The old input format raises a deprecation warning, so catch it here so
# it doesn't cause py.test to fail.
with warns_deprecated_sympy():
KM.kanes_equations(FL, BL)
MM = KM.mass_matrix
forcing = KM.forcing
rhs = MM.inv() * forcing
assert expand(rhs[0]) == expand(-(q * k + u * c) / m)
assert simplify(KM.rhs() -
KM.mass_matrix_full.LUsolve(KM.forcing_full)) == zeros(
2, 1)
assert (KM.linearize(A_and_B=True, )[0] == Matrix([[0, 1],
[-k / m, -c / m]]))
def __init__(self, inlist):
"""This is the constructor for the Vector class. You shouldn't be
calling this, it should only be used by other functions. You should be
treating Vectors like you would with if you were doing the math by
hand, and getting the first 3 from the standard basis vectors from a
ReferenceFrame.
The only exception is to create a zero vector:
zv = Vector(0)
"""
self.args = []
if inlist == 0:
inlist = []
while len(inlist) != 0:
added = 0
for i, v in enumerate(self.args):
if inlist[0][1] == self.args[i][1]:
self.args[i] = (self.args[i][0] + inlist[0][0],
inlist[0][1])
inlist.remove(inlist[0])
added = 1
break
if added != 1:
self.args.append(inlist[0])
inlist.remove(inlist[0])
i = 0
# This code is to remove empty frames from the list
while i < len(self.args):
if self.args[i][0] == Matrix([0, 0, 0]):
self.args.remove(self.args[i])
i -= 1
i += 1
def _w_diff_dcm(self, otherframe):
"""Angular velocity from time differentiating the DCM. """
from sympy.physics.vector.functions import dynamicsymbols
dcm2diff = otherframe.dcm(self)
diffed = dcm2diff.diff(dynamicsymbols._t)
angvelmat = diffed * dcm2diff.T
w1 = trigsimp(expand(angvelmat[7]), recursive=True)
w2 = trigsimp(expand(angvelmat[2]), recursive=True)
w3 = trigsimp(expand(angvelmat[3]), recursive=True)
return Vector([(Matrix([w1, w2, w3]), otherframe)])
def test_form_3():
symsystem3 = SymbolicSystem(
states,
dyn_implicit_rhs,
mass_matrix=dyn_implicit_mat,
coordinate_derivatives=kin_explicit_rhs,
alg_con=alg_con,
coord_idxs=coord_idxs,
speed_idxs=speed_idxs,
bodies=bodies,
loads=loads,
)
assert symsystem3.coordinates == Matrix([x, y])
assert symsystem3.speeds == Matrix([u, v])
assert symsystem3.states == Matrix([x, y, u, v, lam])
assert symsystem3.alg_con == [4]
inter1 = kin_explicit_rhs
inter2 = dyn_implicit_rhs
assert simplify(symsystem3.kin_explicit_rhs - inter1) == zeros(2, 1)
assert simplify(symsystem3.dyn_implicit_mat - dyn_implicit_mat) == zeros(3)
assert simplify(symsystem3.dyn_implicit_rhs - inter2) == zeros(3, 1)
inter = comb_implicit_rhs
assert simplify(symsystem3.comb_implicit_rhs - inter) == zeros(5, 1)
assert simplify(symsystem3.comb_implicit_mat -
comb_implicit_mat) == zeros(5)
inter = comb_explicit_rhs
symsystem3.compute_explicit_form()
assert simplify(symsystem3.comb_explicit_rhs - inter) == zeros(5, 1)
assert set(symsystem3.dynamic_symbols()) == set([y, v, lam, u, x])
assert type(symsystem3.dynamic_symbols()) == tuple
assert set(symsystem3.constant_symbols()) == set([l, g, m])
assert type(symsystem3.constant_symbols()) == tuple
assert symsystem3.output_eqns == {}
assert symsystem3.bodies == (Pa, )
assert symsystem3.loads == ((P, g * m * N.x), )
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
def test_find_dynamicsymbols():
a, b = symbols('a, b')
x, y, z = dynamicsymbols('x, y, z')
expr = Matrix([[a * x + b, x * y.diff() + y],
[x.diff().diff(), z + sin(z.diff())]])
# Test finding all dynamicsymbols
sol = {x, y.diff(), y, x.diff().diff(), z, z.diff()}
assert find_dynamicsymbols(expr) == sol
# Test finding all but those in sym_list
exclude = [x, y, z]
sol = {y.diff(), x.diff().diff(), z.diff()}
assert find_dynamicsymbols(expr, exclude) == sol
def test_msubs():
a, b = symbols('a, b')
x, y, z = dynamicsymbols('x, y, z')
# Test simple substitution
expr = Matrix([[a * x + b, x * y.diff() + y],
[x.diff().diff(), z + sin(z.diff())]])
sol = Matrix([[a + b, y], [x.diff().diff(), 1]])
sd = {x: 1, z: 1, z.diff(): 0, y.diff(): 0}
assert msubs(expr, sd) == sol
# Test smart substitution
expr = cos(x + y) * tan(x + y) + b * x.diff()
sd = {x: 0, y: pi / 2, x.diff(): 1}
assert msubs(expr, sd, smart=True) == b + 1
N = ReferenceFrame('N')
v = x * N.x + y * N.y
d = x * (N.x | N.x) + y * (N.y | N.y)
v_sol = 1 * N.y
d_sol = 1 * (N.y | N.y)
sd = {x: 0, y: 1}
assert msubs(v, sd) == v_sol
assert msubs(d, sd) == d_sol
def _w_diff_dcm(self, otherframe):
"""Angular velocity from time differentiating the DCM. """
from sympy.physics.vector.functions import dynamicsymbols
dcm2diff = self.dcm(otherframe)
diffed = dcm2diff.diff(dynamicsymbols._t)
# angvelmat = diffed * dcm2diff.T
# This one seems to produce the correct result when I checked using Autolev.
angvelmat = dcm2diff * diffed.T
w1 = trigsimp(expand(angvelmat[7]), recursive=True)
w2 = trigsimp(expand(angvelmat[2]), recursive=True)
w3 = trigsimp(expand(angvelmat[3]), recursive=True)
return -Vector([(Matrix([w1, w2, w3]), self)])
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 _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 test_input_format():
# 1 dof problem from test_one_dof
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)
kd = [qd - u]
FL = [(P, (-k * q - c * u) * N.x)]
pa = Particle('pa', P, m)
BL = [pa]
KM = KanesMethod(N, [q], [u], kd)
# test for input format kane.kanes_equations((body1, body2, particle1))
assert KM.kanes_equations(BL)[0] == Matrix([0])
# test for input format kane.kanes_equations(bodies=(body1, body 2), loads=(load1,load2))
assert KM.kanes_equations(bodies=BL, loads=None)[0] == Matrix([0])
# test for input format kane.kanes_equations(bodies=(body1, body 2), loads=None)
assert KM.kanes_equations(BL, loads=None)[0] == Matrix([0])
# test for input format kane.kanes_equations(bodies=(body1, body 2))
assert KM.kanes_equations(BL)[0] == Matrix([0])
# test for input format kane.kanes_equations(bodies=(body1, body2), loads=[])
assert KM.kanes_equations(BL, [])[0] == Matrix([0])
# test for error raised when a wrong force list (in this case a string) is provided
raises(ValueError, lambda: KM._form_fr('bad input'))
# 1 dof problem from test_one_dof with FL & BL in instance
KM = KanesMethod(N, [q], [u], kd, bodies=BL, forcelist=FL)
assert KM.kanes_equations()[0] == Matrix([-c*u - k*q])
# 2 dof problem from test_two_dof
q1, q2, u1, u2 = dynamicsymbols('q1 q2 u1 u2')
q1d, q2d, u1d, u2d = dynamicsymbols('q1 q2 u1 u2', 1)
m, c1, c2, k1, k2 = symbols('m c1 c2 k1 k2')
N = ReferenceFrame('N')
P1 = Point('P1')
P2 = Point('P2')
P1.set_vel(N, u1 * N.x)
P2.set_vel(N, (u1 + u2) * N.x)
kd = [q1d - u1, q2d - u2]
FL = ((P1, (-k1 * q1 - c1 * u1 + k2 * q2 + c2 * u2) * N.x), (P2, (-k2 *
q2 - c2 * u2) * N.x))
pa1 = Particle('pa1', P1, m)
pa2 = Particle('pa2', P2, m)
BL = (pa1, pa2)
KM = KanesMethod(N, q_ind=[q1, q2], u_ind=[u1, u2], kd_eqs=kd)
# test for input format
# kane.kanes_equations((body1, body2), (load1, load2))
KM.kanes_equations(BL, FL)
MM = KM.mass_matrix
forcing = KM.forcing
rhs = MM.inv() * forcing
assert expand(rhs[0]) == expand((-k1 * q1 - c1 * u1 + k2 * q2 + c2 * u2)/m)
assert expand(rhs[1]) == expand((k1 * q1 + c1 * u1 - 2 * k2 * q2 - 2 *
c2 * u2) / m)
def to_matrix(self, reference_frame, second_reference_frame=None):
"""Returns the matrix form of the dyadic with respect to one or two
reference frames.
Parameters
----------
reference_frame : ReferenceFrame
The reference frame that the rows and columns of the matrix
correspond to. If a second reference frame is provided, this
only corresponds to the rows of the matrix.
second_reference_frame : ReferenceFrame, optional, default=None
The reference frame that the columns of the matrix correspond
to.
Returns
-------
matrix : ImmutableMatrix, shape(3,3)
The matrix that gives the 2D tensor form.
Examples
========
>>> from sympy import symbols
>>> from sympy.physics.vector import ReferenceFrame, Vector
>>> Vector.simp = True
>>> from sympy.physics.mechanics import inertia
>>> Ixx, Iyy, Izz, Ixy, Iyz, Ixz = symbols('Ixx, Iyy, Izz, Ixy, Iyz, Ixz')
>>> N = ReferenceFrame('N')
>>> inertia_dyadic = inertia(N, Ixx, Iyy, Izz, Ixy, Iyz, Ixz)
>>> inertia_dyadic.to_matrix(N)
Matrix([
[Ixx, Ixy, Ixz],
[Ixy, Iyy, Iyz],
[Ixz, Iyz, Izz]])
>>> beta = symbols('beta')
>>> A = N.orientnew('A', 'Axis', (beta, N.x))
>>> inertia_dyadic.to_matrix(A)
Matrix([
[ Ixx, Ixy*cos(beta) + Ixz*sin(beta), -Ixy*sin(beta) + Ixz*cos(beta)],
[ Ixy*cos(beta) + Ixz*sin(beta), Iyy*cos(2*beta)/2 + Iyy/2 + Iyz*sin(2*beta) - Izz*cos(2*beta)/2 + Izz/2, -Iyy*sin(2*beta)/2 + Iyz*cos(2*beta) + Izz*sin(2*beta)/2],
[-Ixy*sin(beta) + Ixz*cos(beta), -Iyy*sin(2*beta)/2 + Iyz*cos(2*beta) + Izz*sin(2*beta)/2, -Iyy*cos(2*beta)/2 + Iyy/2 - Iyz*sin(2*beta) + Izz*cos(2*beta)/2 + Izz/2]])
"""
if second_reference_frame is None:
second_reference_frame = reference_frame
return Matrix([
i.dot(self).dot(j) for i in reference_frame
for j in second_reference_frame
]).reshape(3, 3)
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 test_linearize_pendulum_kane_minimal():
q1 = dynamicsymbols('q1') # angle of pendulum
u1 = dynamicsymbols('u1') # Angular velocity
q1d = dynamicsymbols('q1', 1) # Angular velocity
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
A = N.orientnew('A', 'axis', [q1, N.z])
A.set_ang_vel(N, u1 * N.z)
# Locate point P relative to the origin N*
P = pN.locatenew('P', L * A.x)
P.v2pt_theory(pN, N, A)
pP = Particle('pP', P, m)
# Create Kinematic Differential Equations
kde = Matrix([q1d - u1])
# Input the force resultant at P
R = m * g * N.x
# Solve for eom with kanes method
KM = KanesMethod(N, q_ind=[q1], u_ind=[u1], kd_eqs=kde)
with warnings.catch_warnings():
warnings.filterwarnings("ignore", category=SymPyDeprecationWarning)
(fr, frstar) = KM.kanes_equations([(P, R)], [pP])
# Linearize
A, B, inp_vec = KM.linearize(A_and_B=True, simplify=True)
assert A == Matrix([[0, 1], [-9.8 * cos(q1) / L, 0]])
assert B == Matrix([])
def test_linearize_pendulum_kane_minimal():
q1 = dynamicsymbols("q1") # angle of pendulum
u1 = dynamicsymbols("u1") # Angular velocity
q1d = dynamicsymbols("q1", 1) # Angular velocity
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
A = N.orientnew("A", "axis", [q1, N.z])
A.set_ang_vel(N, u1 * N.z)
# Locate point P relative to the origin N*
P = pN.locatenew("P", L * A.x)
P.v2pt_theory(pN, N, A)
pP = Particle("pP", P, m)
# Create Kinematic Differential Equations
kde = Matrix([q1d - u1])
# Input the force resultant at P
R = m * g * N.x
# Solve for eom with kanes method
KM = KanesMethod(N, q_ind=[q1], u_ind=[u1], kd_eqs=kde)
with warns_deprecated_sympy():
(fr, frstar) = KM.kanes_equations([(P, R)], [pP])
# Linearize
A, B, inp_vec = KM.linearize(A_and_B=True, simplify=True)
assert A == Matrix([[0, 1], [-9.8 * cos(q1) / L, 0]])
assert B == Matrix([])
def test_linearize_rolling_disc_lagrange():
q1, q2, q3 = q = dynamicsymbols("q1 q2 q3")
q1d, q2d, q3d = qd = dynamicsymbols("q1 q2 q3", 1)
r, m, g = symbols("r m g")
N = ReferenceFrame("N")
Y = N.orientnew("Y", "Axis", [q1, N.z])
L = Y.orientnew("L", "Axis", [q2, Y.x])
R = L.orientnew("R", "Axis", [q3, L.y])
C = Point("C")
C.set_vel(N, 0)
Dmc = C.locatenew("Dmc", r * L.z)
Dmc.v2pt_theory(C, N, R)
I = inertia(L, m / 4 * r ** 2, m / 2 * r ** 2, m / 4 * r ** 2)
BodyD = RigidBody("BodyD", Dmc, R, m, (I, Dmc))
BodyD.potential_energy = -m * g * r * cos(q2)
Lag = Lagrangian(N, BodyD)
l = LagrangesMethod(Lag, q)
l.form_lagranges_equations()
# Linearize about steady-state upright rolling
op_point = {
q1: 0,
q2: 0,
q3: 0,
q1d: 0,
q2d: 0,
q1d.diff(): 0,
q2d.diff(): 0,
q3d.diff(): 0,
}
A = l.linearize(q_ind=q, qd_ind=qd, op_point=op_point, A_and_B=True)[0]
sol = Matrix(
[
[0, 0, 0, 1, 0, 0],
[0, 0, 0, 0, 1, 0],
[0, 0, 0, 0, 0, 1],
[0, 0, 0, 0, -6 * q3d, 0],
[0, -4 * g / (5 * r), 0, 6 * q3d / 5, 0, 0],
[0, 0, 0, 0, 0, 0],
]
)
assert A == sol