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 warnings.catch_warnings():
warnings.filterwarnings("ignore", category=SymPyDeprecationWarning)
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 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_two_dof():
# This is for a 2 d.o.f., 2 particle spring-mass-damper.
# The first coordinate is the displacement of the first particle, and the
# second is the relative displacement between the first and second
# particles. Speeds are defined as the time derivatives of the particles.
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]
# Now we create the list of forces, then assign properties to each
# particle, then create a list of all particles.
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]
# Finally we create the KanesMethod object, specify the inertial frame,
# pass relevant information, and form Fr & Fr*. Then we calculate the mass
# matrix and forcing terms, and finally solve for the udots.
KM = KanesMethod(N, q_ind=[q1, q2], u_ind=[u1, u2], kd_eqs=kd)
KM.kanes_equations(FL, BL)
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 __init__(self):
#We define some quantities required for tests here..
self.p = dynamicsymbols('p:3')
self.q = dynamicsymbols('q:3')
self.dynamic = list(self.p) + list(self.q)
self.states = [radians(45) for x in self.p] + \
[radians(30) for x in self.q]
self.I = ReferenceFrame('I')
self.A = self.I.orientnew('A', 'space', self.p, 'XYZ')
self.B = self.A.orientnew('B', 'space', self.q, 'XYZ')
self.O = Point('O')
self.P1 = self.O.locatenew('P1', 10 * self.I.x + \
10 * self.I.y + 10 * self.I.z)
self.P2 = self.P1.locatenew('P2', 10 * self.I.x + \
10 * self.I.y + 10 * self.I.z)
self.point_list1 = [[2, 3, 1], [4, 6, 2], [5, 3, 1], [5, 3, 6]]
self.point_list2 = [[3, 1, 4], [3, 8, 2], [2, 1, 6], [2, 1, 1]]
self.shape1 = Cylinder()
self.shape2 = Cylinder()
self.Ixx, self.Iyy, self.Izz = symbols('Ixx Iyy Izz')
self.mass = symbols('mass')
self.parameters = [self.Ixx, self.Iyy, self.Izz, self.mass]
self.param_vals = [0, 0, 0, 0]
self.inertia = inertia(self.A, self.Ixx, self.Iyy, self.Izz)
self.rigid_body = RigidBody('rigid_body1', self.P1, self.A, \
self.mass, (self.inertia, self.P1))
self.global_frame1 = VisualizationFrame('global_frame1', \
self.A, self.P1, self.shape1)
self.global_frame2 = VisualizationFrame('global_frame2', \
self.B, self.P2, self.shape2)
self.scene1 = Scene(self.I, self.O, \
(self.global_frame1, self.global_frame2), \
name='scene')
self.particle = Particle('particle1', self.P1, self.mass)
#To make it more readable
p = self.p
q = self.q
#Here is the dragon ..
self.transformation_matrix = \
[[cos(p[1])*cos(p[2]), sin(p[2])*cos(p[1]), -sin(p[1]), 0], \
[sin(p[0])*sin(p[1])*cos(p[2]) - sin(p[2])*cos(p[0]), \
sin(p[0])*sin(p[1])*sin(p[2]) + cos(p[0])*cos(p[2]), \
sin(p[0])*cos(p[1]), 0], \
[sin(p[0])*sin(p[2]) + sin(p[1])*cos(p[0])*cos(p[2]), \
-sin(p[0])*cos(p[2]) + sin(p[1])*sin(p[2])*cos(p[0]), \
cos(p[0])*cos(p[1]), 0], \
[10, 10, 10, 1]]
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_pend():
q, u = dynamicsymbols('q u')
qd, ud = dynamicsymbols('q u', 1)
m, l, g = symbols('m l g')
N = ReferenceFrame('N')
P = Point('P')
P.set_vel(N, -l * u * sin(q) * N.x + l * u * cos(q) * N.y)
kd = [qd - u]
FL = [(P, m * g * N.x)]
pa = Particle()
pa.mass = m
pa.point = P
BL = [pa]
KM = Kane(N)
KM.coords([q])
KM.speeds([u])
KM.kindiffeq(kd)
KM.kanes_equations(FL, BL)
MM = KM.mass_matrix
forcing = KM.forcing
rhs = MM.inv() * forcing
rhs.simplify()
assert expand(rhs[0]) == expand(-g / l * sin(q))
def test_one_dof():
# This is for a 1 dof spring-mass-damper case.
# It is described in more detail in the Kane 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.mass = m
pa.point = P
BL = [pa]
KM = Kane(N)
KM.coords([q])
KM.speeds([u])
KM.kindiffeq(kd)
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 KM.linearize() == (Matrix([[0, 1], [k, c]]), Matrix([]))
def test_dyadic():
d1 = A.x | A.x
d2 = A.y | A.y
d3 = A.x | A.y
assert d1 * 0 == 0
assert d1 != 0
assert d1 * 2 == 2 * A.x | A.x
assert d1 / 2. == 0.5 * d1
assert d1 & (0 * d1) == 0
assert d1 & d2 == 0
assert d1 & A.x == A.x
assert d1 ^ A.x == 0
assert d1 ^ A.y == A.x | A.z
assert d1 ^ A.z == - A.x | A.y
assert d2 ^ A.x == - A.y | A.z
assert A.x ^ d1 == 0
assert A.y ^ d1 == - A.z | A.x
assert A.z ^ d1 == A.y | A.x
assert A.x & d1 == A.x
assert A.y & d1 == 0
assert A.y & d2 == A.y
assert d1 & d3 == A.x | A.y
assert d3 & d1 == 0
assert d1.dt(A) == 0
q = dynamicsymbols('q')
qd = dynamicsymbols('q', 1)
B = A.orientnew('B', 'Axis', [q, A.z])
assert d1.express(B) == d1.express(B, B)
assert d1.express(B) == ((cos(q)**2) * (B.x | B.x) + (-sin(q) * cos(q)) *
(B.x | B.y) + (-sin(q) * cos(q)) * (B.y | B.x) + (sin(q)**2) *
(B.y | B.y))
assert d1.express(B, A) == (cos(q)) * (B.x | A.x) + (-sin(q)) * (B.y | A.x)
assert d1.express(A, B) == (cos(q)) * (A.x | B.x) + (-sin(q)) * (A.x | B.y)
assert d1.dt(B) == (-qd) * (A.y | A.x) + (-qd) * (A.x | A.y)
def test_parallel_axis():
# This is for a 2 dof inverted pendulum on a cart.
# This tests the parallel axis code in Kane. The inertia of the pendulum is
# defined about the hinge, not about the center of mass.
# Defining the constants and knowns of the system
gravity = symbols('g')
k, ls = symbols('k ls')
a, mA, mC = symbols('a mA mC')
F = dynamicsymbols('F')
Ix, Iy, Iz = symbols('Ix Iy Iz')
# Declaring the Generalized coordinates and speeds
q1, q2 = dynamicsymbols('q1 q2')
q1d, q2d = dynamicsymbols('q1 q2', 1)
u1, u2 = dynamicsymbols('u1 u2')
u1d, u2d = dynamicsymbols('u1 u2', 1)
# Creating reference frames
N = ReferenceFrame('N')
A = ReferenceFrame('A')
A.orient(N, 'Axis', [-q2, N.z])
A.set_ang_vel(N, -u2 * N.z)
# Origin of Newtonian reference frame
O = Point('O')
# Creating and Locating the positions of the cart, C, and the
# center of mass of the pendulum, A
C = O.locatenew('C', q1 * N.x)
Ao = C.locatenew('Ao', a * A.y)
# Defining velocities of the points
O.set_vel(N, 0)
C.set_vel(N, u1 * N.x)
Ao.v2pt_theory(C, N, A)
Cart = Particle('Cart', C, mC)
Pendulum = RigidBody('Pendulum', Ao, A, mA, (inertia(A, Ix, Iy, Iz), C))
# kinematical differential equations
kindiffs = [q1d - u1, q2d - u2]
bodyList = [Cart, Pendulum]
forceList = [(Ao, -N.y * gravity * mA),
(C, -N.y * gravity * mC),
(C, -N.x * k * (q1 - ls)),
(C, N.x * F)]
km=Kane(N)
km.coords([q1, q2])
km.speeds([u1, u2])
km.kindiffeq(kindiffs)
(fr,frstar) = km.kanes_equations(forceList, bodyList)
mm = km.mass_matrix_full
assert mm[3, 3] == -Iz
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 Kane. 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])
R.set_ang_vel(N, u1 * L.x + u2 * L.y + u3 * L.z)
R.set_ang_acc(N, R.ang_vel_in(N).dt(R) + (R.ang_vel_in(N) ^
R.ang_vel_in(N)))
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 = [q1d - u3/cos(q3), q2d - u1, q3d - u2 + u3 * tan(q2)]
ForceList = [(Dmc, - m * g * Y.z), (C, f1 * L.x + f2 * (Y.z ^ L.x))]
BodyD = RigidBody()
BodyD.mc = Dmc
BodyD.inertia = (I, Dmc)
BodyD.frame = R
BodyD.mass = m
BodyList = [BodyD]
KM = Kane(N)
KM.coords([q1, q2, q3])
KM.speeds([u1, u2, u3, u4, u5])
KM.kindiffeq(kd)
kdd = KM.kindiffdict()
(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 = Kane(N)
KM2.coords([q1, q2, q3])
KM2.speeds([u1, u2, u3], u_auxiliary=[u4, u5])
KM2.kindiffeq(kd)
(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})
assert fr.expand() == fr2.expand()
assert frstar.expand() == frstar2.expand()
def __init__(self):
super(Aircraft, self).__init__()
# states
body__x, body__v_x, body__a_x = mech.dynamicsymbols('body.x, body.v_x, body.a_x')
self.x = sympy.Matrix([body__x, body__v_x, body__a_x])
self.x0 = {
body__x : 0.0,
body__v_x : 0.0,
body__a_x : 0.0,
}
# variables
accel__ma_x, accel__b_x__y = mech.dynamicsymbols('accel.ma_x, accel.b_x.y')
self.v = sympy.Matrix([accel__ma_x, accel__b_x__y])
# constants
self.c = sympy.Matrix([])
self.c0 = {
}
# parameters
body__g, body__c, body__m, accel__b_x__b = sympy.symbols('body.g, body.c, body.m, accel.b_x.b')
self.p = sympy.Matrix([body__g, body__c, body__m, accel__b_x__b])
self.p0 = {
body__g : 9.81,
body__c : 0.9,
body__m : 1.0,
accel__b_x__b : 0.0,
}
# inputs
body__f_x, accel__a_x, accel__b_x__u = mech.dynamicsymbols('body.f_x, accel.a_x, accel.b_x.u')
self.u = sympy.Matrix([body__f_x, accel__a_x, accel__b_x__u])
self.u0 = {
body__f_x : 0.0,
accel__a_x : 0.0,
accel__b_x__u : 0.0,
}
# outputs
body__x, body__v_x, body__a_x, accel__ma_x, accel__b_x__y = mech.dynamicsymbols('body.x, body.v_x, body.a_x, accel.ma_x, accel.b_x.y')
self.y = sympy.Matrix([body__x, body__v_x, body__a_x, accel__ma_x, accel__b_x__y])
# equations
self.eqs = [
body__a_x - (accel__a_x),
(body__x).diff(self.t) - (body__v_x),
(body__v_x).diff(self.t) - (body__a_x),
body__f_x - (body__m * body__a_x),
accel__b_x__u - (accel__a_x),
accel__ma_x - (accel__b_x__y),
accel__b_x__y - (accel__b_x__u + accel__b_x__b),
]
self.compute_fg()
def test_kin_eqs():
q0, q1, q2, q3 = dynamicsymbols('q0 q1 q2 q3')
q0d, q1d, q2d, q3d = dynamicsymbols('q0 q1 q2 q3', 1)
u1, u2, u3 = dynamicsymbols('u1 u2 u3')
kds = kinematic_equations([u1, u2, u3], [q0, q1, q2, q3], 'quaternion')
assert kds == [-0.5 * q0 * u1 - 0.5 * q2 * u3 + 0.5 * q3 * u2 + q1d,
-0.5 * q0 * u2 + 0.5 * q1 * u3 - 0.5 * q3 * u1 + q2d,
-0.5 * q0 * u3 - 0.5 * q1 * u2 + 0.5 * q2 * u1 + q3d,
0.5 * q1 * u1 + 0.5 * q2 * u2 + 0.5 * q3 * u3 + q0d]
def test_integrate(self):
times = np.linspace(0, 1, 100)
# Try without calling generate_ode_function.
# ------------------------------------------
sys = System(self.kane, times=times)
x_01 = sys.integrate()
sys = System(self.kane, times=times)
sys.generate_ode_function(generator='lambdify')
x_02 = sys.integrate()
testing.assert_allclose(x_01, x_02)
# Ensure that the defaults are as expected.
# -----------------------------------------
constants_dict = dict(zip(sm.symbols('m0, k0, c0, g'),
[1.0, 1.0, 1.0, 1.0]))
specified_dict = {dynamicsymbols('f0'): 0.0}
x_03 = sys.ode_solver(sys.evaluate_ode_function, [0, 0], sys.times,
args=(specified_dict, constants_dict))
testing.assert_allclose(x_02, x_03)
# Ensure that initial conditions are reordered properly.
# ------------------------------------------------------
sys = System(self.kane, times=times)
# I know that this is the order of the states.
x0 = [5.1, 3.7]
ic = {dynamicsymbols('x0'): x0[0], dynamicsymbols('v0'): x0[1]}
sys.initial_conditions = ic
x_04 = sys.integrate()
x_05 = sys.ode_solver(
sys.evaluate_ode_function, x0, sys.times,
args=(sys._specifieds_padded_with_defaults(),
sys._constants_padded_with_defaults()))
testing.assert_allclose(x_04, x_05)
# Test a generator other than lambdify.
# -------------------------------------
if theano:
sys.generate_ode_function(generator='theano')
sys.times = times
x_06 = sys.integrate()
testing.assert_allclose(x_04, x_06)
else:
warnings.warn("Theano was not found so the related tests are being"
" skipped.", PyDyImportWarning)
# Unrecognized generator.
# -----------------------
sys = System(self.kane, times=times)
with testing.assert_raises(NotImplementedError):
sys.generate_ode_function(generator='made-up')
def test_parallel_axis():
# This is for a 2 dof inverted pendulum on a cart.
# This tests the parallel axis code in KanesMethod. The inertia of the
# pendulum is defined about the hinge, not about the center of mass.
# Defining the constants and knowns of the system
gravity = symbols("g")
k, ls = symbols("k ls")
a, mA, mC = symbols("a mA mC")
F = dynamicsymbols("F")
Ix, Iy, Iz = symbols("Ix Iy Iz")
# Declaring the Generalized coordinates and speeds
q1, q2 = dynamicsymbols("q1 q2")
q1d, q2d = dynamicsymbols("q1 q2", 1)
u1, u2 = dynamicsymbols("u1 u2")
u1d, u2d = dynamicsymbols("u1 u2", 1)
# Creating reference frames
N = ReferenceFrame("N")
A = ReferenceFrame("A")
A.orient(N, "Axis", [-q2, N.z])
A.set_ang_vel(N, -u2 * N.z)
# Origin of Newtonian reference frame
O = Point("O")
# Creating and Locating the positions of the cart, C, and the
# center of mass of the pendulum, A
C = O.locatenew("C", q1 * N.x)
Ao = C.locatenew("Ao", a * A.y)
# Defining velocities of the points
O.set_vel(N, 0)
C.set_vel(N, u1 * N.x)
Ao.v2pt_theory(C, N, A)
Cart = Particle("Cart", C, mC)
Pendulum = RigidBody("Pendulum", Ao, A, mA, (inertia(A, Ix, Iy, Iz), C))
# kinematical differential equations
kindiffs = [q1d - u1, q2d - u2]
bodyList = [Cart, Pendulum]
forceList = [(Ao, -N.y * gravity * mA), (C, -N.y * gravity * mC), (C, -N.x * k * (q1 - ls)), (C, N.x * F)]
km = KanesMethod(N, [q1, q2], [u1, u2], kindiffs)
with warnings.catch_warnings():
warnings.filterwarnings("ignore", category=SymPyDeprecationWarning)
(fr, frstar) = km.kanes_equations(forceList, bodyList)
mm = km.mass_matrix_full
assert mm[3, 3] == Iz
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_rolling_disc():
# Rolling Disc Example
# Here the rolling disc is formed from the contact point up, removing the
# need to introduce generalized speeds. Only 3 configuration and 3
# speed variables are need to describe this system, along with the
# disc's mass and radius, and the local gravity.
q1, q2, q3 = dynamicsymbols('q1 q2 q3')
q1d, q2d, q3d = dynamicsymbols('q1 q2 q3', 1)
r, m, g = symbols('r m g')
# The kinematics are formed by a series of simple rotations. Each simple
# rotation creates a new frame, and the next rotation is defined by the new
# frame's basis vectors. This example uses a 3-1-2 series of rotations, or
# Z, X, Y series of rotations. Angular velocity for this is defined using
# the second frame's basis (the lean frame).
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])
# This is the translational kinematics. We create a point with no velocity
# in N; this is the contact point between the disc and ground. Next we form
# the position vector from the contact point to the disc's center of mass.
# Finally we form the velocity and acceleration of the disc.
C = Point('C')
C.set_vel(N, 0)
Dmc = C.locatenew('Dmc', r * L.z)
Dmc.v2pt_theory(C, N, R)
# Forming the inertia dyadic.
I = inertia(L, m / 4 * r**2, m / 2 * r**2, m / 4 * r**2)
BodyD = RigidBody('BodyD', Dmc, R, m, (I, Dmc))
# Finally we form the equations of motion, using the same steps we did
# before. Supply the Lagrangian, the generalized speeds.
BodyD.set_potential_energy(- m * g * r * cos(q2))
Lag = Lagrangian(N, BodyD)
q = [q1, q2, q3]
q1 = Function('q1')
q2 = Function('q2')
q3 = Function('q3')
l = LagrangesMethod(Lag, q)
l.form_lagranges_equations()
RHS = l.rhs()
RHS.simplify()
t = symbols('t')
assert (l.mass_matrix[3:6] == [0, 5*m*r**2/4, 0])
assert RHS[4].simplify() == (-8*g*sin(q2(t)) + 5*r*sin(2*q2(t)
)*Derivative(q1(t), t)**2 + 12*r*cos(q2(t))*Derivative(q1(t), t
)*Derivative(q3(t), t))/(10*r)
assert RHS[5] == (-5*cos(q2(t))*Derivative(q1(t), t) + 6*tan(q2(t)
)*Derivative(q3(t), t) + 4*Derivative(q1(t), t)/cos(q2(t))
)*Derivative(q2(t), t)
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.utilities.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_get_motion_methods():
#Initialization
t = dynamicsymbols._t
s1, s2, s3 = symbols('s1 s2 s3')
S1, S2, S3 = symbols('S1 S2 S3')
S4, S5, S6 = symbols('S4 S5 S6')
t1, t2 = symbols('t1 t2')
a, b, c = dynamicsymbols('a b c')
ad, bd, cd = dynamicsymbols('a b c', 1)
a2d, b2d, c2d = dynamicsymbols('a b c', 2)
v0 = S1*N.x + S2*N.y + S3*N.z
v01 = S4*N.x + S5*N.y + S6*N.z
v1 = s1*N.x + s2*N.y + s3*N.z
v2 = a*N.x + b*N.y + c*N.z
v2d = ad*N.x + bd*N.y + cd*N.z
v2dd = a2d*N.x + b2d*N.y + c2d*N.z
#Test position parameter
assert get_motion_params(frame = N) == (0, 0, 0)
assert get_motion_params(N, position=v1) == (0, 0, v1)
assert get_motion_params(N, position=v2) == (v2dd, v2d, v2)
#Test velocity parameter
assert get_motion_params(N, velocity=v1) == (0, v1, v1 * t)
assert get_motion_params(N, velocity=v1, position=v0, timevalue1=t1) == \
(0, v1, v0 + v1*(t - t1))
answer = get_motion_params(N, velocity=v1, position=v2, timevalue1=t1)
answer_expected = (0, v1, v1*t - v1*t1 + v2.subs(t, t1))
assert answer == answer_expected
answer = get_motion_params(N, velocity=v2, position=v0, timevalue1=t1)
integral_vector = Integral(a, (t, t1, t))*N.x + Integral(b, (t, t1, t))*N.y \
+ Integral(c, (t, t1, t))*N.z
answer_expected = (v2d, v2, v0 + integral_vector)
assert answer == answer_expected
#Test acceleration parameter
assert get_motion_params(N, acceleration=v1) == (v1, v1 * t, v1 * t**2/2)
assert get_motion_params(N, acceleration=v1, velocity=v0,
position=v2, timevalue1=t1, timevalue2=t2) == \
(v1, (v0 + v1*t - v1*t2),
-v0*t1 + v1*t**2/2 + v1*t2*t1 - \
v1*t1**2/2 + t*(v0 - v1*t2) + \
v2.subs(t, t1))
assert get_motion_params(N, acceleration=v1, velocity=v0,
position=v01, timevalue1=t1, timevalue2=t2) == \
(v1, v0 + v1*t - v1*t2,
-v0*t1 + v01 + v1*t**2/2 + \
v1*t2*t1 - v1*t1**2/2 + \
t*(v0 - v1*t2))
answer = get_motion_params(N, acceleration=a*N.x, velocity=S1*N.x,
position=S2*N.x, timevalue1=t1, timevalue2=t2)
i1 = Integral(a, (t, t2, t))
answer_expected = (a*N.x, (S1 + i1)*N.x, \
(S2 + Integral(S1 + i1, (t, t1, t)))*N.x)
assert answer == answer_expected
def test_point_a2pt_theorys():
q = dynamicsymbols('q')
qd = dynamicsymbols('q', 1)
qdd = dynamicsymbols('q', 2)
N = ReferenceFrame('N')
B = N.orientnew('B', 'Axis', [q, N.z])
O = Point('O')
P = O.locatenew('P', 0)
O.set_vel(N, 0)
assert P.a2pt_theory(O, N, B) == 0
P.set_pos(O, B.x)
assert P.a2pt_theory(O, N, B) == (-qd**2) * B.x + (qdd) * B.y
def test_dyadic():
d1 = A.x | A.x
d2 = A.y | A.y
d3 = A.x | A.y
assert d1 * 0 == 0
assert d1 != 0
assert d1 * 2 == 2 * A.x | A.x
assert d1 / 2. == 0.5 * d1
assert d1 & (0 * d1) == 0
assert d1 & d2 == 0
assert d1 & A.x == A.x
assert d1 ^ A.x == 0
assert d1 ^ A.y == A.x | A.z
assert d1 ^ A.z == - A.x | A.y
assert d2 ^ A.x == - A.y | A.z
assert A.x ^ d1 == 0
assert A.y ^ d1 == - A.z | A.x
assert A.z ^ d1 == A.y | A.x
assert A.x & d1 == A.x
assert A.y & d1 == 0
assert A.y & d2 == A.y
assert d1 & d3 == A.x | A.y
assert d3 & d1 == 0
assert d1.dt(A) == 0
q = dynamicsymbols('q')
qd = dynamicsymbols('q', 1)
B = A.orientnew('B', 'Axis', [q, A.z])
assert d1.express(B) == d1.express(B, B)
assert d1.express(B) == ((cos(q)**2) * (B.x | B.x) + (-sin(q) * cos(q)) *
(B.x | B.y) + (-sin(q) * cos(q)) * (B.y | B.x) + (sin(q)**2) *
(B.y | B.y))
assert d1.express(B, A) == (cos(q)) * (B.x | A.x) + (-sin(q)) * (B.y | A.x)
assert d1.express(A, B) == (cos(q)) * (A.x | B.x) + (-sin(q)) * (A.x | B.y)
assert d1.dt(B) == (-qd) * (A.y | A.x) + (-qd) * (A.x | A.y)
assert d1.to_matrix(A) == Matrix([[1, 0, 0], [0, 0, 0], [0, 0, 0]])
assert d1.to_matrix(A, B) == Matrix([[cos(q), -sin(q), 0],
[0, 0, 0],
[0, 0, 0]])
assert d3.to_matrix(A) == Matrix([[0, 1, 0], [0, 0, 0], [0, 0, 0]])
a, b, c, d, e, f = symbols('a, b, c, d, e, f')
v1 = a * A.x + b * A.y + c * A.z
v2 = d * A.x + e * A.y + f * A.z
d4 = v1.outer(v2)
assert d4.to_matrix(A) == Matrix([[a * d, a * e, a * f],
[b * d, b * e, b * f],
[c * d, c * e, c * f]])
d5 = v1.outer(v1)
C = A.orientnew('C', 'Axis', [q, A.x])
for expected, actual in zip(C.dcm(A) * d5.to_matrix(A) * C.dcm(A).T,
d5.to_matrix(C)):
assert (expected - actual).simplify() == 0
def test_integrate(self):
times = np.linspace(0, 1, 100)
# Try without calling generate_ode_function.
# ------------------------------------------
sys = System(self.kane, times=times)
x_01 = sys.integrate()
sys = System(self.kane, times=times)
rhs = sys.generate_ode_function(generator='lambdify')
x_02 = sys.integrate()
testing.assert_allclose(x_01, x_02)
# Ensure that the defaults are as expected.
# -----------------------------------------
constants_dict = dict(zip(symbols('m, k, c, g'), [1.0, 1.0, 1.0, 1.0]))
specified_dict = {dynamicsymbols('F'): 0.0}
x_03 = sys.ode_solver(sys.evaluate_ode_function, [0, 0], sys.times,
args=({
'constants': constants_dict,
'specified': specified_dict},))
testing.assert_allclose(x_02, x_03)
# Ensure that initial conditions are reordered properly.
# ------------------------------------------------------
sys = System(self.kane, times=times)
# I know that this is the order of the states.
x0 = [5.1, 3.7]
ic = {dynamicsymbols('x'): x0[0], dynamicsymbols('v'): x0[1]}
sys.initial_conditions = ic
x_04 = sys.integrate()
x_05 = sys.ode_solver(sys.evaluate_ode_function,
x0, sys.times, args=(
{'constants': sys._constants_padded_with_defaults(),
'specified': sys._specifieds_padded_with_defaults()},))
testing.assert_allclose(x_04, x_05)
# Test a generator other than lambdify.
# -------------------------------------
sys.generate_ode_function(generator='theano')
sys.times = times
x_06 = sys.integrate()
testing.assert_allclose(x_04, x_06)
# Unrecognized generator.
# -----------------------
sys = System(self.kane, times=times)
with testing.assert_raises(NotImplementedError):
sys.generate_ode_function(generator='made-up')
def test_point_v2pt_theorys():
q = dynamicsymbols('q')
qd = dynamicsymbols('q', 1)
N = ReferenceFrame('N')
B = N.orientnew('B', 'Axis', [q, N.z])
O = Point('O')
P = O.locatenew('P', 0)
O.set_vel(N, 0)
assert P.v2pt_theory(O, N, B) == 0
P = O.locatenew('P', B.x)
assert P.v2pt_theory(O, N, B) == (qd * B.z ^ B.x)
O.set_vel(N, N.x)
assert P.v2pt_theory(O, N, B) == N.x + qd * B.y
# -*- coding: utf-8 -*-
"""
Spyder Editor
Author: Chul Lee
"""
import scipy as sci
import numpy as np
#import plot.matplotlib as plt
import sympy as sym
from sympy.physics.mechanics import dynamicsymbols
"""Question 1"""
t, u, v, w = dynamicsymbols(
't u v w') #declares t u v w as symbols for differentiation
t, u, v, w
var = ['t', 'u', 'v', 'w']
#z = [1,1,1,1] #initial guess
#t=z[0]
#u=z[1]
#v=z[2]
#w=z[3]
#vec = np.array(z) #sets up vector
#vec[0]=t**4+u**4-1
#vec[1]=t**2-u**2+1
#vec[2]=v**4+w**4-1
#vec[3]=v**2-w**2+1
f1 = t**4 + u**4 - 1
f2 = t**2 - u**2 + 1
f3 = v**4 + w**4 - 1
f4 = v**2 - w**2 + 1
#!/usr/bin/env python
# -*- coding: utf-8 -*-
"""Exercise 8.6 from Kane 1985."""
from __future__ import division
from sympy import cos, sin, solve, simplify, symbols
from sympy.physics.mechanics import ReferenceFrame, Point
from sympy.physics.mechanics import dynamicsymbols
from util import msprint, subs, partial_velocities, generalized_active_forces
## --- Declare symbols ---
# Define the system with 6 generalized speeds as follows:
q1, q2, q3 = dynamicsymbols('q1:4')
q1d, q2d, q3d = dynamicsymbols('q1:4', level=1)
u1, u2, u3 = dynamicsymbols('u1:4')
L1, L2, L3, L4 = symbols('L1:5')
g, m1, m2, t = symbols('g m1 m2 t')
# --- ReferenceFrames ---
A = ReferenceFrame('A')
# --- Define Points and set their velocities ---
pO = Point('O')
pO.set_vel(A, 0)
pP1 = pO.locatenew('P1', L1 * (cos(q1) * A.x + sin(q1) * A.y))
pP1.set_vel(A, pP1.pos_from(pO).diff(t, A))
pP2 = pP1.locatenew('P2', L2 * (cos(q2) * A.x + sin(q2) * A.y))
pP2.set_vel(A, pP2.pos_from(pO).diff(t, A))
## --- configuration constraints ---
cc = [
#!/usr/bin/env python
# coding: utf-8
# In[295]:
#Escrito por Alfredo Aguiar 30 de diciembre del 2020.
# importamos las librerías necesarias
import sympy as sp # librería para cálculo simbólico
from sympy.physics.vector import init_vprinting
init_vprinting(use_latex='mathjax', pretty_print=False)
from sympy.physics.mechanics import dynamicsymbols
theta1, theta2, l1, l2, theta, alpha, a, d = dynamicsymbols(
'theta1 theta2 l1 l2 theta alpha a d')
#Definimos las matrices de rotacion y traslacion correspondientes
#Rotacion en Z
rz = sp.Matrix([[sp.cos(theta), -sp.sin(theta), 0, 0],
[sp.sin(theta), sp.cos(theta), 0, 0], [0, 0, 1, 0],
[0, 0, 0, 1]])
#Traslacion en Z
tz = sp.Matrix([[1, 0, 0, 0], [0, 1, 0, 0], [0, 0, 1, d], [0, 0, 0, 1]])
#Traslacion en X
tx = sp.Matrix([[1, 0, 0, a], [0, 1, 0, 0], [0, 0, 1, 0], [0, 0, 0, 1]])
#Rotacion en X
import sympy.physics.mechanics as me
import sympy as sm
import math as m
import numpy as np
x, y = me.dynamicsymbols('x y')
xd, yd = me.dynamicsymbols('x y', 1)
e1 = (x + y)**2 + (x - y)**3
e2 = (x - y)**2
e3 = x**2 + y**2 + 2 * x * y
m1 = sm.Matrix([e1, e2]).reshape(2, 1)
m2 = sm.Matrix([(x + y)**2, (x - y)**2]).reshape(1, 2)
m3 = m1 + sm.Matrix([x, y]).reshape(2, 1)
am = sm.Matrix([i.expand() for i in m1]).reshape((m1).shape[0], (m1).shape[1])
cm = sm.Matrix([
i.expand() for i in sm.Matrix([(x + y)**2, (x - y)**2]).reshape(1, 2)
]).reshape((sm.Matrix([(x + y)**2, (x - y)**2]).reshape(1, 2)).shape[0],
(sm.Matrix([(x + y)**2, (x - y)**2]).reshape(1, 2)).shape[1])
em = sm.Matrix([i.expand() for i in m1 + sm.Matrix([x, y]).reshape(2, 1)
]).reshape((m1 + sm.Matrix([x, y]).reshape(2, 1)).shape[0],
(m1 + sm.Matrix([x, y]).reshape(2, 1)).shape[1])
f = (e1).expand()
g = (e2).expand()
a = sm.factor((e3), x)
bm = sm.Matrix([sm.factor(i, x) for i in m1]).reshape((m1).shape[0],
(m1).shape[1])
cm = sm.Matrix([sm.factor(i, x) for i in m1 + sm.Matrix([x, y]).reshape(2, 1)
]).reshape((m1 + sm.Matrix([x, y]).reshape(2, 1)).shape[0],
(m1 + sm.Matrix([x, y]).reshape(2, 1)).shape[1])
a = (e3).diff(x)
b = (e3).diff(y)
import sympy.physics.mechanics as me
import sympy as sm
import math as m
import numpy as np
x1, x2 = me.dynamicsymbols("x1 x2")
f1 = x1 * x2 + 3 * x1**2
f2 = x1 * me.dynamicsymbols._t + x2 * me.dynamicsymbols._t**2
x, y = me.dynamicsymbols("x y")
xd, yd = me.dynamicsymbols("x y", 1)
yd2 = me.dynamicsymbols("y", 2)
q1, q2, q3, u1, u2 = me.dynamicsymbols("q1 q2 q3 u1 u2")
p1, p2 = me.dynamicsymbols("p1 p2")
p1d, p2d = me.dynamicsymbols("p1 p2", 1)
w1, w2, w3, r1, r2 = me.dynamicsymbols("w1 w2 w3 r1 r2")
w1d, w2d, w3d, r1d, r2d = me.dynamicsymbols("w1 w2 w3 r1 r2", 1)
r1d2, r2d2 = me.dynamicsymbols("r1 r2", 2)
c11, c12, c21, c22 = me.dynamicsymbols("c11 c12 c21 c22")
d11, d12, d13 = me.dynamicsymbols("d11 d12 d13")
j1, j2 = me.dynamicsymbols("j1 j2")
n = sm.symbols("n")
n = sm.I
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)
(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 == Matrix([[0, 1], [-9.8 / L, 0]])
assert B == Matrix([])
def test_rolling_disc():
# Rolling Disc Example
# Here the rolling disc is formed from the contact point up, removing the
# need to introduce generalized speeds. Only 3 configuration and three
# speed variables are need to describe this system, along with the disc's
# mass and radius, and the local gravity (note that mass will drop out).
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)
r, m, g = symbols('r m g')
# The kinematics are formed by a series of simple rotations. Each simple
# rotation creates a new frame, and the next rotation is defined by the new
# frame's basis vectors. This example uses a 3-1-2 series of rotations, or
# Z, X, Y series of rotations. Angular velocity for this is defined using
# the second frame's basis (the lean frame).
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])
R.set_ang_vel(N, u1 * L.x + u2 * L.y + u3 * L.z)
R.set_ang_acc(N,
R.ang_vel_in(N).dt(R) + (R.ang_vel_in(N) ^ R.ang_vel_in(N)))
# This is the translational kinematics. We create a point with no velocity
# in N; this is the contact point between the disc and ground. Next we form
# the position vector from the contact point to the disc's center of mass.
# Finally we form the velocity and acceleration of the disc.
C = Point('C')
C.set_vel(N, 0)
Dmc = C.locatenew('Dmc', r * L.z)
Dmc.v2pt_theory(C, N, R)
Dmc.a2pt_theory(C, N, R)
# This is a simple way to form the inertia dyadic.
I = inertia(L, m / 4 * r**2, m / 2 * r**2, m / 4 * r**2)
# Kinematic differential equations; how the generalized coordinate time
# derivatives relate to generalized speeds.
kd = [q1d - u3 / cos(q2), q2d - u1, q3d - u2 + u3 * tan(q2)]
# Creation of the force list; it is the gravitational force at the mass
# center of the disc. Then we create the disc by assigning a Point to the
# center of mass attribute, a ReferenceFrame to the frame attribute, and mass
# and inertia. Then we form the body list.
ForceList = [(Dmc, -m * g * Y.z)]
BodyD = RigidBody('BodyD', Dmc, R, m, (I, Dmc))
BodyList = [BodyD]
# Finally we form the equations of motion, using the same steps we did
# before. Specify inertial frame, supply generalized speeds, supply
# kinematic differential equation dictionary, compute Fr from the force
# list and Fr* from the body list, compute the mass matrix and forcing
# terms, then solve for the u dots (time derivatives of the generalized
# speeds).
KM = KanesMethod(N, q_ind=[q1, q2, q3], u_ind=[u1, u2, u3], kd_eqs=kd)
KM.kanes_equations(ForceList, BodyList)
MM = KM.mass_matrix
forcing = KM.forcing
rhs = MM.inv() * forcing
kdd = KM.kindiffdict()
rhs = rhs.subs(kdd)
assert rhs.expand() == Matrix([
(10 * u2 * u3 * r - 5 * u3**2 * r * tan(q2) + 4 * g * sin(q2)) /
(5 * r), -2 * u1 * u3 / 3, u1 * (-2 * u2 + u3 * tan(q2))
]).expand()
import sympy.physics.mechanics as me
import sympy as sm
import math as m
import numpy as np
frame_a = me.ReferenceFrame('a')
frame_b = me.ReferenceFrame('b')
q1, q2, q3 = me.dynamicsymbols('q1 q2 q3')
frame_b.orient(frame_a, 'Axis', [q3, frame_a.x])
dcm = frame_a.dcm(frame_b)
m = dcm * 3 - frame_a.dcm(frame_b)
r = me.dynamicsymbols('r')
circle_area = sm.pi * r**2
u, a = me.dynamicsymbols('u a')
x, y = me.dynamicsymbols('x y')
s = u * me.dynamicsymbols._t - 1 / 2 * a * me.dynamicsymbols._t**2
expr1 = 2 * a * 0.5 - 1.25 + 0.25
expr2 = -1 * x**2 + y**2 + 0.25 * (x + y)**2
expr3 = 0.5 * 10**(-10)
dyadic = me.outer(frame_a.x, frame_a.x) + me.outer(
frame_a.y, frame_a.y) + me.outer(frame_a.z, frame_a.z)
# # Dynamic Model of a Cart with a (not-inverted) pendulum (2D)
# +
from sympy import *
from sympy.physics.mechanics import dynamicsymbols, init_vprinting
init_vprinting()
# Time and input variable
t, u = symbols('t u')
# Physical parameters
m, M, l, g = symbols('m M l g')
# Load angle
alpha = dynamicsymbols('\talpha')
# Angular Velocity
alphadot = diff(alpha, t)
# Angular Acceleration
alphaddot = diff(alphadot, t)
# -
# ## Coordinates $(x,\alpha)$
# +
# Position
x = dynamicsymbols('x')
xL = x - l * sin(alpha) # xL as function of x
zL = -l * cos(alpha) # zL as function of z
potential_energy, LagrangesMethod, mechanics_printing)
from sympy.utilities.lambdify import lambdify
from sympy.simplify import trigsimp
import parameters as par
# For shorter expressions
mechanics_printing(pretty_print=False)
# ----------------------------------------------------------------------------
# Define symbols
# ----------------------------------------------------------------------------
# Generalized coordinates
n_coords = 3
q = dynamicsymbols("q:" + str(n_coords))
dq = dynamicsymbols("q:" + str(n_coords), level=1)
# Mass and link lengths
m_links = symbols("m_l:2")
d_links = symbols("d_l:2")
m_point = symbols("m_cart m_A m_B m_C") # for point objects
b_cart, b_joint = symbols("b_cart b_joint") # Viscous damping
g, t = symbols("g, t") # General parameters
k, l0 = symbols("k l0") # Spring parameters
F = symbols("F") # Input force; both motor and hydraulic brake
# Lists to store the objects
particles = []
def main():
# model_name = 'F2T0RNA'
#model_name = 'F2T0RNA_fnd'
# model_name = 'F2T0N0S1_fnd'
# model_name = 'F2T1RNA'
# model_name = 'F2T1RNA_fnd'
# model_name = 'F3T1RNA_fnd'
# model_name = 'F5T1RNA_fnd'
# model_name = 'F2T1N0S1_fnd'
# model_name = 'F0T2RNA'
# model_name = 'F0T1RNA'
# model_name = 'F0T2N0S1'
# model_name = 'F6T0RNA'
#model_name = 'F5T0N0S1_fnd'
#
# model_name = 'F6T0RNA_fnd'
# model_name = 'F6T0N0S1'
# model_name = 'F6T1RNA'
#model_name = 'F6T1RNA_fnd'
# model_name = 'F6T1N0S1'
# model_name = 'F6T1N0S1_fnd'
# model_name = 'F6T2N0S1'
#model_name='F000101T0N0S1_fnd'
# model_name='F000111T0N0S1_fnd'
# model_name='F000111T0RNA_fnd'
# model_name='F000101T0RNA_fnd'
# model_name='B000101'
# model_name='B000011'
# model_name='B000111'
# Models=['F2T1N0S1_fnd', 'F6T1N0S1_fnd', 'F6T1N0S1']
# Models=['F6T1N0S1_fnd', 'F6T1N0S1']
# Models=['F6T1N0S1_fnd']
# Models=[ 'F2T0RNA_fnd' ,'F2T0N0S1_fnd' ,'F2T1RNA_fnd' ,'F3T1RNA_fnd' ,'F5T1RNA_fnd' ,'F2T1N0S1' ,'F0T2RNA' ,'F0T1RNA' ,'F0T2N0S1' ,'F6T0RNA' ,'F5T0N0S1_fnd' ,'F6T0RNA_fnd' ,'F6T0N0S1' ,'F6T1RNA' ,'F6T1RNA_fnd' ]
#Models=['F2T0RNA' , 'F2T0RNA_fnd', 'F2T0N0S1' , 'F2T1RNA' , 'F2T1N0S1' , 'F0T2RNA' , 'F0T2N0S1' , 'F6T0RNA' , 'F6T0RNA_fnd', 'F6T0N0S1' , 'F6T1RNA' , 'F6T1N0S1' , 'F6T2N0S1' ]
# Models=['F2T0RNA' , 'F2T0RNA_fnd']
# Models=['F6T1RNA']
Models = ['F6T1N0S1'] #, 'F5T1N0S1_fnd']
Models = ['F0T2N0S1'] #, 'F5T1N0S1_fnd']
for model_name in Models:
# if True:
bSmallAngle = True
opts = dict()
opts[
'rot_elastic_type'] = 'SmallRot' #<<< Very important, 'SmallRot', or 'Body', will affect the rotation matrix
# opts['rot_elastic_type']='Body' #<<< Very important, 'SmallRot', or 'Body', will affect the rotation matrix
opts[
'rot_elastic_smallAngle'] = False #<<< Very important, will perform small angle approx: sin(nu q) = nu q and nu^2=0 !!! Will remove all nu^2 and nu^3 terms!! Not recommended, removes part of RNA "Y" inertia from mass matrix
opts['orderMM'] = 1 #< order of taylor expansion for Mass Matrix
opts['orderH'] = 1 #< order of taylor expansion for H term
opts['fnd_loads'] = False
opts['aero_torques'] = False
opts['mergeFndTwr'] = model_name.find('_fnd') <= 0
opts['yaw'] = 'zero' # 'fixed', 'zero', or 'dynamic' if a DOF
opts['tilt'] = 'fixed' # 'fixed', 'zero', or 'dynamic' if a DOF
opts['tiltShaft'] = True # OpenFAST tilts shaft not nacelle
#opts['linRot'] = False # Linearize rotations matrices from the beginning
opts[
'linRot'] = True # Linearize rotations matrices from the beginning
opts['Mform'] = 'symbolic' # or 'TaylorExpanded'
opts['twrDOFDir'] = [
'x', 'y', 'x', 'y'
] # Order in which the flexible DOF of the tower are set
# --- Esthetics Replacements for python
replaceDict = {}
replaceDict['theta_tilt'] = ('tilt', None)
# --- Create model, solve equations and preform small angle approximation
if model_name[0] == 'B':
model = get_model_one_body(model_name, **opts)
else:
model = get_model(model_name, **opts)
model.kaneEquations(Mform='TaylorExpanded')
# ---
extraSubs = model.shapeNormSubs
# extraSubs+=[(Symbol('J_xx_T'),0)]
# extraSubs+=[(Symbol('J_yy_T'),0)]
# extraSubs+=[(Symbol('J_zz_T'),0)]
# extraSubs+=[(Symbol('J_xx_N'),0)]
# extraSubs+=[(Symbol('J_yy_N'),0)]
# extraSubs+=[(Symbol('J_zz_N'),0)]
# extraSubs+=[(diff(Symbol('phi'),time),0)]
# extraSubs+=[(Symbol('M_N'),0)]
#if model_name[0]=='B':
#extraSubs+=[(Symbol('x_BG'),0)]
#extraSubs+=[(Symbol('y_BG'),0)]
print('Extra Subs: ', extraSubs)
# --- Linearization of non linear equations
model.linearize(noAcc=True, noVel=False, extraSubs=extraSubs)
# --- Small angle approximation and linearization
if bSmallAngle:
print('Small angles:', model.smallAngles)
model.smallAngleApprox(model.smallAngles, extraSubs)
model.smallAngleApproxEOM(model.smallAngles, extraSubs)
model.smallAngleLinearize(noAcc=True,
noVel=False,
extraSubs=extraSubs)
model.savePython(folder='_py',
variables=[
'MM', 'FF', 'MMsa', 'FFsa', 'M', 'C', 'K',
'B', 'Msa', 'Csa', 'Ksa', 'Bsa'
],
replaceDict=replaceDict,
extraSubs=extraSubs)
else:
model.savePython(folder='_py',
variables=['MM', 'FF', 'M', 'C', 'K', 'B'],
replaceDict=replaceDict,
extraSubs=extraSubs,
doSimplify=False)
# --- No vel
print('>>>>>> NO VEL')
velSubs = [(qd, 0)
for q, qd in zip(model.coordinates, model.coordinates_speed)
if q is not dynamicsymbols('psi')]
velSubs += [(qd, 0) for q, qd in zip(model.coordinates, model.speeds)
if q is not dynamicsymbols('psi')]
print('>>>', extraSubs)
if bSmallAngle:
model.savePython(folder='_py',
prefix='noVel_SA',
variables=['Msa', 'Csa', 'Ksa', 'Bsa'],
replaceDict=replaceDict,
extraSubs=extraSubs,
doSimplify=True,
velSubs=velSubs)
model.saveTex(folder='_tex',
prefix='noVel_SA',
variables=['Msa', 'Csa', 'Ksa', 'Bsa'],
extraHeader='NoVelocity: ',
header=True,
doSimplify=True,
velSubs=velSubs)
def test_non_central_inertia():
# This tests that the calculation of Fr* does not depend the point
# about which the inertia of a rigid body is defined. This test solves
# exercises 8.12, 8.17 from Kane 1985.
# Declare symbols
q1, q2, q3 = dynamicsymbols('q1:4')
q1d, q2d, q3d = dynamicsymbols('q1:4', level=1)
u1, u2, u3, u4, u5 = dynamicsymbols('u1:6')
u_prime, R, M, g, e, f, theta = symbols('u\' R, M, g, e, f, theta')
a, b, mA, mB, IA, J, K, t = symbols('a b mA mB IA J K t')
Q1, Q2, Q3 = symbols('Q1, Q2 Q3')
IA22, IA23, IA33 = symbols('IA22 IA23 IA33')
# Reference Frames
F = ReferenceFrame('F')
P = F.orientnew('P', 'axis', [-theta, F.y])
A = P.orientnew('A', 'axis', [q1, P.x])
A.set_ang_vel(F, u1 * A.x + u3 * A.z)
# define frames for wheels
B = A.orientnew('B', 'axis', [q2, A.z])
C = A.orientnew('C', 'axis', [q3, A.z])
B.set_ang_vel(A, u4 * A.z)
C.set_ang_vel(A, u5 * A.z)
# define points D, S*, Q on frame A and their velocities
pD = Point('D')
pD.set_vel(A, 0)
# u3 will not change v_D_F since wheels are still assumed to roll without slip.
pD.set_vel(F, u2 * A.y)
pS_star = pD.locatenew('S*', e * A.y)
pQ = pD.locatenew('Q', f * A.y - R * A.x)
for p in [pS_star, pQ]:
p.v2pt_theory(pD, F, A)
# masscenters of bodies A, B, C
pA_star = pD.locatenew('A*', a * A.y)
pB_star = pD.locatenew('B*', b * A.z)
pC_star = pD.locatenew('C*', -b * A.z)
for p in [pA_star, pB_star, pC_star]:
p.v2pt_theory(pD, F, A)
# points of B, C touching the plane P
pB_hat = pB_star.locatenew('B^', -R * A.x)
pC_hat = pC_star.locatenew('C^', -R * A.x)
pB_hat.v2pt_theory(pB_star, F, B)
pC_hat.v2pt_theory(pC_star, F, C)
# the velocities of B^, C^ are zero since B, C are assumed to roll without slip
kde = [q1d - u1, q2d - u4, q3d - u5]
vc = [dot(p.vel(F), A.y) for p in [pB_hat, pC_hat]]
# inertias of bodies A, B, C
# IA22, IA23, IA33 are not specified in the problem statement, but are
# necessary to define an inertia object. Although the values of
# IA22, IA23, IA33 are not known in terms of the variables given in the
# problem statement, they do not appear in the general inertia terms.
inertia_A = inertia(A, IA, IA22, IA33, 0, IA23, 0)
inertia_B = inertia(B, K, K, J)
inertia_C = inertia(C, K, K, J)
# define the rigid bodies A, B, C
rbA = RigidBody('rbA', pA_star, A, mA, (inertia_A, pA_star))
rbB = RigidBody('rbB', pB_star, B, mB, (inertia_B, pB_star))
rbC = RigidBody('rbC', pC_star, C, mB, (inertia_C, pC_star))
km = KanesMethod(F,
q_ind=[q1, q2, q3],
u_ind=[u1, u2],
kd_eqs=kde,
u_dependent=[u4, u5],
velocity_constraints=vc,
u_auxiliary=[u3])
forces = [(pS_star, -M * g * F.x), (pQ, Q1 * A.x + Q2 * A.y + Q3 * A.z)]
bodies = [rbA, rbB, rbC]
with warns_deprecated_sympy():
fr, fr_star = km.kanes_equations(forces, bodies)
vc_map = solve(vc, [u4, u5])
# KanesMethod returns the negative of Fr, Fr* as defined in Kane1985.
fr_star_expected = Matrix([
-(IA + 2 * J * b**2 / R**2 + 2 * K + mA * a**2 + 2 * mB * b**2) *
u1.diff(t) - mA * a * u1 * u2,
-(mA + 2 * mB + 2 * J / R**2) * u2.diff(t) + mA * a * u1**2, 0
])
t = trigsimp(fr_star.subs(vc_map).subs({u3: 0})).doit().expand()
assert ((fr_star_expected - t).expand() == zeros(3, 1))
# define inertias of rigid bodies A, B, C about point D
# I_S/O = I_S/S* + I_S*/O
bodies2 = []
for rb, I_star in zip([rbA, rbB, rbC], [inertia_A, inertia_B, inertia_C]):
I = I_star + inertia_of_point_mass(rb.mass, rb.masscenter.pos_from(pD),
rb.frame)
bodies2.append(RigidBody('', rb.masscenter, rb.frame, rb.mass,
(I, pD)))
with warns_deprecated_sympy():
fr2, fr_star2 = km.kanes_equations(forces, bodies2)
t = trigsimp(fr_star2.subs(vc_map).subs({u3: 0})).doit()
assert (fr_star_expected - t).expand() == zeros(3, 1)
from sympy.core.backend import symbols, Matrix, atan, zeros
from sympy import simplify
from sympy.physics.mechanics import (
dynamicsymbols,
Particle,
Point,
ReferenceFrame,
SymbolicSystem,
)
from sympy.testing.pytest import raises
# This class is going to be tested using a simple pendulum set up in x and y
# coordinates
x, y, u, v, lam = dynamicsymbols("x y u v lambda")
m, l, g = symbols("m l g")
# Set up the different forms the equations can take
# [1] Explicit form where the kinematics and dynamics are combined
# x' = F(x, t, r, p)
#
# [2] Implicit form where the kinematics and dynamics are combined
# M(x, p) x' = F(x, t, r, p)
#
# [3] Implicit form where the kinematics and dynamics are separate
# M(q, p) u' = F(q, u, t, r, p)
# q' = G(q, u, t, r, p)
dyn_implicit_mat = Matrix([[1, 0, -x / m], [0, 1, -y / m], [0, 0, l**2 / m]])
dyn_implicit_rhs = Matrix([0, 0, u**2 + v**2 - g * y])
comb_implicit_mat = Matrix([
def multi_mass_spring_damper(n=1,
apply_gravity=False,
apply_external_forces=False):
r"""Returns a system containing the symbolic equations of motion and
associated variables for a simple multi-degree of freedom point mass,
spring, damper system with optional gravitational and external
specified forces. For example, a two mass system under the influence of
gravity and external forces looks like:
::
----------------
| | | | g
\ | | | V
k0 / --- c0 |
| | | x0, v0
--------- V
| m0 | -----
--------- |
| | | |
\ v | | |
k1 / f0 --- c1 |
| | | x1, v1
--------- V
| m1 | -----
---------
| f1
V
Parameters
==========
n : integer
The number of masses in the serial chain.
apply_gravity : boolean
If true, gravity will be applied to each mass.
apply_external_forces : boolean
If true, a time varying external force will be applied to each mass.
Returns
=======
kane : sympy.physics.mechanics.kane.KanesMethod
A KanesMethod object.
"""
mass = sm.symbols('m:{}'.format(n))
stiffness = sm.symbols('k:{}'.format(n))
damping = sm.symbols('c:{}'.format(n))
acceleration_due_to_gravity = sm.symbols('g')
coordinates = me.dynamicsymbols('x:{}'.format(n))
speeds = me.dynamicsymbols('v:{}'.format(n))
specifieds = me.dynamicsymbols('f:{}'.format(n))
ceiling = me.ReferenceFrame('N')
origin = me.Point('origin')
origin.set_vel(ceiling, 0)
points = [origin]
kinematic_equations = []
particles = []
forces = []
for i in range(n):
center = points[-1].locatenew('center{}'.format(i),
coordinates[i] * ceiling.x)
center.set_vel(ceiling,
points[-1].vel(ceiling) + speeds[i] * ceiling.x)
points.append(center)
block = me.Particle('block{}'.format(i), center, mass[i])
kinematic_equations.append(speeds[i] - coordinates[i].diff())
total_force = (-stiffness[i] * coordinates[i] - damping[i] * speeds[i])
try:
total_force += (stiffness[i + 1] * coordinates[i + 1] +
damping[i + 1] * speeds[i + 1])
except IndexError: # no force from below on last mass
pass
if apply_gravity:
total_force += mass[i] * acceleration_due_to_gravity
if apply_external_forces:
total_force += specifieds[i]
forces.append((center, total_force * ceiling.x))
particles.append(block)
kane = me.KanesMethod(ceiling,
q_ind=coordinates,
u_ind=speeds,
kd_eqs=kinematic_equations)
kane.kanes_equations(particles, forces)
return kane
def test_rolling_disc():
# Rolling Disc Example
# Here the rolling disc is formed from the contact point up, removing the
# need to introduce generalized speeds. Only 3 configuration and three
# speed variables are need to describe this system, along with the disc's
# mass and radius, and the local gravity (note that mass will drop out).
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)
r, m, g = symbols('r m g')
# The kinematics are formed by a series of simple rotations. Each simple
# rotation creates a new frame, and the next rotation is defined by the new
# frame's basis vectors. This example uses a 3-1-2 series of rotations, or
# Z, X, Y series of rotations. Angular velocity for this is defined using
# the second frame's basis (the lean frame).
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)
# This is the translational kinematics. We create a point with no velocity
# in N; this is the contact point between the disc and ground. Next we form
# the position vector from the contact point to the disc's center of mass.
# Finally we form the velocity and acceleration of the disc.
C = Point('C')
C.set_vel(N, 0)
Dmc = C.locatenew('Dmc', r * L.z)
Dmc.v2pt_theory(C, N, R)
# This is a simple way to form the inertia dyadic.
I = inertia(L, m / 4 * r**2, m / 2 * r**2, m / 4 * r**2)
# Kinematic differential equations; how the generalized coordinate time
# derivatives relate to generalized speeds.
kd = [dot(R.ang_vel_in(N) - w_R_N_qd, uv) for uv in L]
# Creation of the force list; it is the gravitational force at the mass
# center of the disc. Then we create the disc by assigning a Point to the
# center of mass attribute, a ReferenceFrame to the frame attribute, and mass
# and inertia. Then we form the body list.
ForceList = [(Dmc, -m * g * Y.z)]
BodyD = RigidBody('BodyD', Dmc, R, m, (I, Dmc))
BodyList = [BodyD]
# Finally we form the equations of motion, using the same steps we did
# before. Specify inertial frame, supply generalized speeds, supply
# kinematic differential equation dictionary, compute Fr from the force
# list and Fr* from the body list, compute the mass matrix and forcing
# terms, then solve for the u dots (time derivatives of the generalized
# speeds).
KM = KanesMethod(N, q_ind=[q1, q2, q3], u_ind=[u1, u2, u3], kd_eqs=kd)
with warns_deprecated_sympy():
KM.kanes_equations(ForceList, BodyList)
MM = KM.mass_matrix
forcing = KM.forcing
rhs = MM.inv() * forcing
kdd = KM.kindiffdict()
rhs = rhs.subs(kdd)
rhs.simplify()
assert rhs.expand() == Matrix([
(6 * u2 * u3 * r - u3**2 * r * tan(q2) + 4 * g * sin(q2)) / (5 * r),
-2 * u1 * u3 / 3, u1 * (-2 * u2 + u3 * tan(q2))
]).expand()
assert simplify(KM.rhs() -
KM.mass_matrix_full.LUsolve(KM.forcing_full)) == zeros(
6, 1)
# This code tests our output vs. benchmark values. When r=g=m=1, the
# critical speed (where all eigenvalues of the linearized equations are 0)
# is 1 / sqrt(3) for the upright case.
A = KM.linearize(A_and_B=True)[0]
A_upright = A.subs({
r: 1,
g: 1,
m: 1
}).subs({
q1: 0,
q2: 0,
q3: 0,
u1: 0,
u3: 0
})
import sympy
assert sympy.sympify(A_upright.subs({u2: 1 / sqrt(3)})).eigenvals() == {
S.Zero: 6
}
def n_link_pendulum_on_cart(n=1, cart_force=True, joint_torques=False):
r"""Returns the system containing the symbolic first order equations of
motion for a 2D n-link pendulum on a sliding cart under the influence of
gravity.
::
|
o y v
\ 0 ^ g
\ |
--\-|----
| \| |
F-> | o --|---> x
| |
---------
o o
Parameters
==========
n : integer
The number of links in the pendulum.
cart_force : boolean, default=True
If true an external specified lateral force is applied to the cart.
joint_torques : boolean, default=False
If true joint torques will be added as specified inputs at each
joint.
Returns
=======
kane : sympy.physics.mechanics.kane.KanesMethod
A KanesMethod object.
Notes
=====
The degrees of freedom of the system are n + 1, i.e. one for each
pendulum link and one for the lateral motion of the cart.
M x' = F, where x = [u0, ..., un+1, q0, ..., qn+1]
The joint angles are all defined relative to the ground where the x axis
defines the ground line and the y axis points up. The joint torques are
applied between each adjacent link and the between the cart and the
lower link where a positive torque corresponds to positive angle.
"""
if n <= 0:
raise ValueError('The number of links must be a positive integer.')
q = me.dynamicsymbols('q:{}'.format(n + 1))
u = me.dynamicsymbols('u:{}'.format(n + 1))
if joint_torques is True:
T = me.dynamicsymbols('T1:{}'.format(n + 1))
m = sm.symbols('m:{}'.format(n + 1))
l = sm.symbols('l:{}'.format(n))
g, t = sm.symbols('g t')
I = me.ReferenceFrame('I')
O = me.Point('O')
O.set_vel(I, 0)
P0 = me.Point('P0')
P0.set_pos(O, q[0] * I.x)
P0.set_vel(I, u[0] * I.x)
Pa0 = me.Particle('Pa0', P0, m[0])
frames = [I]
points = [P0]
particles = [Pa0]
forces = [(P0, -m[0] * g * I.y)]
kindiffs = [q[0].diff(t) - u[0]]
if cart_force is True or joint_torques is True:
specified = []
else:
specified = None
for i in range(n):
Bi = I.orientnew('B{}'.format(i), 'Axis', [q[i + 1], I.z])
Bi.set_ang_vel(I, u[i + 1] * I.z)
frames.append(Bi)
Pi = points[-1].locatenew('P{}'.format(i + 1), l[i] * Bi.y)
Pi.v2pt_theory(points[-1], I, Bi)
points.append(Pi)
Pai = me.Particle('Pa' + str(i + 1), Pi, m[i + 1])
particles.append(Pai)
forces.append((Pi, -m[i + 1] * g * I.y))
if joint_torques is True:
specified.append(T[i])
if i == 0:
forces.append((I, -T[i] * I.z))
if i == n - 1:
forces.append((Bi, T[i] * I.z))
else:
forces.append((Bi, T[i] * I.z - T[i + 1] * I.z))
kindiffs.append(q[i + 1].diff(t) - u[i + 1])
if cart_force is True:
F = me.dynamicsymbols('F')
forces.append((P0, F * I.x))
specified.append(F)
kane = me.KanesMethod(I, q_ind=q, u_ind=u, kd_eqs=kindiffs)
kane.kanes_equations(particles, forces)
return kane
def THETA(qtd):
theta = [0]
for i in range(1,qtd+1):
theta.append(dynamicsymbols('theta'+str(i)))
return theta
#!/usr/bin/env python
# -*- coding: utf-8 -*-
"""Exercise 10.3 from Kane 1985."""
from __future__ import division
from sympy import collect, expand, sin, cos, pi, radsimp, solve, sqrt, symbols
from sympy.physics.mechanics import ReferenceFrame, RigidBody, Point
from sympy.physics.mechanics import dot, dynamicsymbols, inertia, msprint
q1, q2, q3, q4, q5, q6, q7 = q = dynamicsymbols('q1:8')
u1, u2, u3, u4, u5, u6, u7 = u = dynamicsymbols('q1:8', level=1)
M, J, I11, I22, m, r, b = symbols('M J I11 I22 m r b',
real=True,
positive=True)
omega, t = symbols('ω t')
theta = 30 * pi / 180 # radians
b = r * (1 + sin(theta)) / (cos(theta) - sin(theta))
# Note: using b as found in Ex3.10. Pure rolling between spheres and race R
# is likely a typo and should be between spheres and cone C.
# define reference frames
R = ReferenceFrame('R') # fixed race rf, let R.z point upwards
A = R.orientnew('A', 'axis', [q7, R.z]) # rf that rotates with S* about R.z
# B.x, B.z are parallel with face of cone, B.y is perpendicular
B = A.orientnew('B', 'axis', [-theta, A.x])
S = ReferenceFrame('S')
S.set_ang_vel(A, u1 * A.x + u2 * A.y + u3 * A.z)
C = ReferenceFrame('C')
C.set_ang_vel(A, u4 * B.x + u5 * B.y + u6 * B.z)
def test_sub_qdot():
# This test solves exercises 8.12, 8.17 from Kane 1985 and defines
# some velocities in terms of q, qdot.
## --- Declare symbols ---
q1, q2, q3 = dynamicsymbols('q1:4')
q1d, q2d, q3d = dynamicsymbols('q1:4', level=1)
u1, u2, u3 = dynamicsymbols('u1:4')
u_prime, R, M, g, e, f, theta = symbols('u\' R, M, g, e, f, theta')
a, b, mA, mB, IA, J, K, t = symbols('a b mA mB IA J K t')
IA22, IA23, IA33 = symbols('IA22 IA23 IA33')
Q1, Q2, Q3 = symbols('Q1 Q2 Q3')
# --- Reference Frames ---
F = ReferenceFrame('F')
P = F.orientnew('P', 'axis', [-theta, F.y])
A = P.orientnew('A', 'axis', [q1, P.x])
A.set_ang_vel(F, u1 * A.x + u3 * A.z)
# define frames for wheels
B = A.orientnew('B', 'axis', [q2, A.z])
C = A.orientnew('C', 'axis', [q3, A.z])
## --- define points D, S*, Q on frame A and their velocities ---
pD = Point('D')
pD.set_vel(A, 0)
# u3 will not change v_D_F since wheels are still assumed to roll w/o slip
pD.set_vel(F, u2 * A.y)
pS_star = pD.locatenew('S*', e * A.y)
pQ = pD.locatenew('Q', f * A.y - R * A.x)
# masscenters of bodies A, B, C
pA_star = pD.locatenew('A*', a * A.y)
pB_star = pD.locatenew('B*', b * A.z)
pC_star = pD.locatenew('C*', -b * A.z)
for p in [pS_star, pQ, pA_star, pB_star, pC_star]:
p.v2pt_theory(pD, F, A)
# points of B, C touching the plane P
pB_hat = pB_star.locatenew('B^', -R * A.x)
pC_hat = pC_star.locatenew('C^', -R * A.x)
pB_hat.v2pt_theory(pB_star, F, B)
pC_hat.v2pt_theory(pC_star, F, C)
# --- relate qdot, u ---
# the velocities of B^, C^ are zero since B, C are assumed to roll w/o slip
kde = [dot(p.vel(F), A.y) for p in [pB_hat, pC_hat]]
kde += [u1 - q1d]
kde_map = solve(kde, [q1d, q2d, q3d])
for k, v in list(kde_map.items()):
kde_map[k.diff(t)] = v.diff(t)
# inertias of bodies A, B, C
# IA22, IA23, IA33 are not specified in the problem statement, but are
# necessary to define an inertia object. Although the values of
# IA22, IA23, IA33 are not known in terms of the variables given in the
# problem statement, they do not appear in the general inertia terms.
inertia_A = inertia(A, IA, IA22, IA33, 0, IA23, 0)
inertia_B = inertia(B, K, K, J)
inertia_C = inertia(C, K, K, J)
# define the rigid bodies A, B, C
rbA = RigidBody('rbA', pA_star, A, mA, (inertia_A, pA_star))
rbB = RigidBody('rbB', pB_star, B, mB, (inertia_B, pB_star))
rbC = RigidBody('rbC', pC_star, C, mB, (inertia_C, pC_star))
## --- use kanes method ---
km = KanesMethod(F, [q1, q2, q3], [u1, u2], kd_eqs=kde, u_auxiliary=[u3])
forces = [(pS_star, -M * g * F.x), (pQ, Q1 * A.x + Q2 * A.y + Q3 * A.z)]
bodies = [rbA, rbB, rbC]
# Q2 = -u_prime * u2 * Q1 / sqrt(u2**2 + f**2 * u1**2)
# -u_prime * R * u2 / sqrt(u2**2 + f**2 * u1**2) = R / Q1 * Q2
fr_expected = Matrix([
f * Q3 + M * g * e * sin(theta) * cos(q1),
Q2 + M * g * sin(theta) * sin(q1),
e * M * g * cos(theta) - Q1 * f - Q2 * R
])
#Q1 * (f - u_prime * R * u2 / sqrt(u2**2 + f**2 * u1**2)))])
fr_star_expected = Matrix([
-(IA + 2 * J * b**2 / R**2 + 2 * K + mA * a**2 + 2 * mB * b**2) *
u1.diff(t) - mA * a * u1 * u2,
-(mA + 2 * mB + 2 * J / R**2) * u2.diff(t) + mA * a * u1**2, 0
])
with warns_deprecated_sympy():
fr, fr_star = km.kanes_equations(forces, bodies)
assert (fr.expand() == fr_expected.expand())
assert ((fr_star_expected - trigsimp(fr_star)).expand() == zeros(3, 1))
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)
(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}
#this file is used to generate a serialized function that can be used to estimate
# next states given current states
#ASSUMES GRAVITY SUFFICIENTLY COMPENSATED BY ROBOT
init_vprinting(use_latex='mathjax', pretty_print=True)
#Kinematics ------------------------------
#init reference frames, assume base attached to floor
inertial_frame = ReferenceFrame('I')
j0_frame = ReferenceFrame('J0')
j1_frame = ReferenceFrame('J1')
j2_frame = ReferenceFrame('J2')
#declare dynamic symbols for the three joints
theta0, theta1, theta2 = dynamicsymbols('theta0, theta1, theta2')
#orient frames
j0_frame.orient(inertial_frame, 'Axis', (theta0, inertial_frame.y))
j1_frame.orient(j0_frame, 'Axis', (theta1, j0_frame.z))
j2_frame.orient(j1_frame, 'Axis', (theta2, j1_frame.z))
#TODO figure out better name for joint points
#init joints
joint0 = Point('j0')
j0_length = symbols('l_j0')
joint1 = Point('j1')
joint1.set_pos(joint0, j0_length * j0_frame.y)
j1_length = symbols('l_j1')
joint2 = Point('j2')
joint2.set_pos(joint1, j1_length * j1_frame.y)
# %%
import sympy as sp
from sympy.physics.mechanics import (dynamicsymbols, ReferenceFrame, Point,
Particle, KanesMethod)
# %%
n = 2
q = dynamicsymbols('q:' + str(n + 1)) # Generalized coordinates
u = dynamicsymbols('u:' + str(n + 1)) # Generalized speeds
f = dynamicsymbols('f') # Force applied to the cart
m = sp.symbols('m:' + str(n + 1)) # Mass of each bob
# %%
import intersection from intersection1
import selectphi from selecting_phi
#jointlimits
#theta_1_u=
#theta_1_l=
#theta_2_u=
#theta_2_l=
#theta_3_u=
#theta_3_l=
#theta_4_u=
#theta_4_l=
#theta_5_u=
#theta_5_l=
#theta_6_u=
#theta_6_l=
alpha,beta,gamma,p_x,p_y,p_z,l_bs,l_se,l_we,l_wt,theta_1,theta_2,theta_3,theta_4,theta_5,theta_6,u_x,u_y,u_z,theta_11,theta_22,theta_33,theta_44,alpha_1,beta_2,phi=dynamicsymbols('alpha,beta,gamma,p_x,p_y,p_z,l_bs,l_se,l_we,l_wt,theta_1,theta_2,theta_3,theta_4,theta_5,theta_6,u_x,u_y,u_z,theta_11,theta_22,theta_33,theta_44,alpha_1,beta_2,phi')
rot_end=sp.Matrix([[sp.cos(beta), -sp.sin(beta)*sp.cos(gamma), sp.sin(beta)*sp.sin(gamma)],
[sp.sin(beta)*sp.cos(alpha), (sp.cos(alpha)*sp.cos(beta)*sp.cos(gamma)-sp.sin(alpha)*sp.sin(gamma)), (-sp.cos(alpha)*sp.cos(beta)*sp.sin(gamma)-sp.sin(alpha)*sp.cos(gamma))],
[sp.sin(alpha)*sp.sin(beta), (sp.cos(alpha)*sp.sin(gamma)+ sp.cos(beta)*sp.cos(gamma)*sin(alpha)), (sp.cos(alpha)*sp.cos(gamma)-sp.cos(beta)*sp.sin(alpha)*sin(gamma))]])
#Euler angle XZX for orientation#
#print(rot_end[0,2])
pos_end=sp.Matrix([[p_x],[p_y],[p_z]])
#print(pos_end.shape)
w=np.zeros((3,1))
l_wt=sp.Matrix([[0],[0],[l_wt]])
w=pos_end-(rot_end*(l_wt))
w_x=w[0,0]
w_y=w[1,0]
w_z=w[2,0]
L_bs=sp.Matrix([[0],[0],[l_bs]])
def L(qtd):
l = [0]
for i in range(1,qtd+1):
l.append(dynamicsymbols('l'+str(i)))
return l
#!/usr/bin/env python
# -*- coding: utf-8 -*-
"""Exercise 10.5 from Kane 1985.
Answer does not match text.
"""
from __future__ import division
from sympy import expand, symbols, trigsimp, sin, cos, S
from sympy.physics.mechanics import ReferenceFrame, RigidBody, Point
from sympy.physics.mechanics import dynamicsymbols, inertia, msprint
m, a, b = symbols('m a b')
q1, q2 = dynamicsymbols('q1, q2')
q1d, q2d = dynamicsymbols('q1, q2', level=1)
# reference frames
# N.x parallel to horizontal line, N.y parallel to line AC
N = ReferenceFrame('N')
A = N.orientnew('A', 'axis', [-q1, N.y])
B = A.orientnew('B', 'axis', [-q2, A.x])
# points B*, O
pO = Point('O')
pB_star = pO.locatenew('B*', S(1) / 3 * (2 * a * B.x - b * B.y))
pO.set_vel(N, 0)
pB_star.v2pt_theory(pO, N, B)
# rigidbody B
I_B_Bs = inertia(B, m * b**2 / 18, m * a**2 / 18, m * (a**2 + b**2) / 18)
rbB = RigidBody('rbB', pB_star, B, m, (I_B_Bs, pB_star))
#!/usr/bin/env python
from sympy import symbols
import sympy.physics.mechanics as me
print("Defining the problem.")
# The conical pendulum will have three links and three bobs.
n = 3
# Each link's orientation is described by two spaced fixed angles: alpha and
# beta.
# Generalized coordinates
alpha = me.dynamicsymbols('alpha:{}'.format(n))
beta = me.dynamicsymbols('beta:{}'.format(n))
# Generalized speeds
omega = me.dynamicsymbols('omega:{}'.format(n))
delta = me.dynamicsymbols('delta:{}'.format(n))
# At each joint there are point masses (i.e. the bobs).
m_bob = symbols('m:{}'.format(n))
# Each link is modeled as a cylinder so it will have a length, mass, and a
# symmetric inertia tensor.
l = symbols('l:{}'.format(n))
m_link = symbols('M:{}'.format(n))
Ixx = symbols('Ixx:{}'.format(n))
Iyy = symbols('Iyy:{}'.format(n))
Izz = symbols('Izz:{}'.format(n))
def test_aux_dep():
# This test is about rolling disc dynamics, comparing the results found
# with KanesMethod to those found when deriving the equations "manually"
# with SymPy.
# The terms Fr, Fr*, and Fr*_steady are all compared between the two
# methods. Here, Fr*_steady refers to the generalized inertia forces for an
# equilibrium configuration.
# Note: comparing to the test of test_rolling_disc() in test_kane.py, this
# test also tests auxiliary speeds and configuration and motion constraints
#, seen in the generalized dependent coordinates q[3], and depend speeds
# u[3], u[4] and u[5].
# First, mannual derivation of Fr, Fr_star, Fr_star_steady.
# Symbols for time and constant parameters.
# Symbols for contact forces: Fx, Fy, Fz.
t, r, m, g, I, J = symbols('t r m g I J')
Fx, Fy, Fz = symbols('Fx Fy Fz')
# Configuration variables and their time derivatives:
# q[0] -- yaw
# q[1] -- lean
# q[2] -- spin
# q[3] -- dot(-r*B.z, A.z) -- distance from ground plane to disc center in
# A.z direction
# Generalized speeds and their time derivatives:
# u[0] -- disc angular velocity component, disc fixed x direction
# u[1] -- disc angular velocity component, disc fixed y direction
# u[2] -- disc angular velocity component, disc fixed z direction
# u[3] -- disc velocity component, A.x direction
# u[4] -- disc velocity component, A.y direction
# u[5] -- disc velocity component, A.z direction
# Auxiliary generalized speeds:
# ua[0] -- contact point auxiliary generalized speed, A.x direction
# ua[1] -- contact point auxiliary generalized speed, A.y direction
# ua[2] -- contact point auxiliary generalized speed, A.z direction
q = dynamicsymbols('q:4')
qd = [qi.diff(t) for qi in q]
u = dynamicsymbols('u:6')
ud = [ui.diff(t) for ui in u]
ud_zero = dict(zip(ud, [0.]*len(ud)))
ua = dynamicsymbols('ua:3')
ua_zero = dict(zip(ua, [0.]*len(ua)))
# Reference frames:
# Yaw intermediate frame: A.
# Lean intermediate frame: B.
# Disc fixed frame: C.
N = ReferenceFrame('N')
A = N.orientnew('A', 'Axis', [q[0], N.z])
B = A.orientnew('B', 'Axis', [q[1], A.x])
C = B.orientnew('C', 'Axis', [q[2], B.y])
# Angular velocity and angular acceleration of disc fixed frame
# u[0], u[1] and u[2] are generalized independent speeds.
C.set_ang_vel(N, u[0]*B.x + u[1]*B.y + u[2]*B.z)
C.set_ang_acc(N, C.ang_vel_in(N).diff(t, B)
+ cross(B.ang_vel_in(N), C.ang_vel_in(N)))
# Velocity and acceleration of points:
# Disc-ground contact point: P.
# Center of disc: O, defined from point P with depend coordinate: q[3]
# u[3], u[4] and u[5] are generalized dependent speeds.
P = Point('P')
P.set_vel(N, ua[0]*A.x + ua[1]*A.y + ua[2]*A.z)
O = P.locatenew('O', q[3]*A.z + r*sin(q[1])*A.y)
O.set_vel(N, u[3]*A.x + u[4]*A.y + u[5]*A.z)
O.set_acc(N, O.vel(N).diff(t, A) + cross(A.ang_vel_in(N), O.vel(N)))
# Kinematic differential equations:
# Two equalities: one is w_c_n_qd = C.ang_vel_in(N) in three coordinates
# directions of B, for qd0, qd1 and qd2.
# the other is v_o_n_qd = O.vel(N) in A.z direction for qd3.
# Then, solve for dq/dt's in terms of u's: qd_kd.
w_c_n_qd = qd[0]*A.z + qd[1]*B.x + qd[2]*B.y
v_o_n_qd = O.pos_from(P).diff(t, A) + cross(A.ang_vel_in(N), O.pos_from(P))
kindiffs = Matrix([dot(w_c_n_qd - C.ang_vel_in(N), uv) for uv in B] +
[dot(v_o_n_qd - O.vel(N), A.z)])
qd_kd = solve(kindiffs, qd)
# Values of generalized speeds during a steady turn for later substitution
# into the Fr_star_steady.
steady_conditions = solve(kindiffs.subs({qd[1] : 0, qd[3] : 0}), u)
steady_conditions.update({qd[1] : 0, qd[3] : 0})
# Partial angular velocities and velocities.
partial_w_C = [C.ang_vel_in(N).diff(ui, N) for ui in u + ua]
partial_v_O = [O.vel(N).diff(ui, N) for ui in u + ua]
partial_v_P = [P.vel(N).diff(ui, N) for ui in u + ua]
# Configuration constraint: f_c, the projection of radius r in A.z direction
# is q[3].
# Velocity constraints: f_v, for u3, u4 and u5.
# Acceleration constraints: f_a.
f_c = Matrix([dot(-r*B.z, A.z) - q[3]])
f_v = Matrix([dot(O.vel(N) - (P.vel(N) + cross(C.ang_vel_in(N),
O.pos_from(P))), ai).expand() for ai in A])
v_o_n = cross(C.ang_vel_in(N), O.pos_from(P))
a_o_n = v_o_n.diff(t, A) + cross(A.ang_vel_in(N), v_o_n)
f_a = Matrix([dot(O.acc(N) - a_o_n, ai) for ai in A])
# Solve for constraint equations in the form of
# u_dependent = A_rs * [u_i; u_aux].
# First, obtain constraint coefficient matrix: M_v * [u; ua] = 0;
# Second, taking u[0], u[1], u[2] as independent,
# taking u[3], u[4], u[5] as dependent,
# rearranging the matrix of M_v to be A_rs for u_dependent.
# Third, u_aux ==0 for u_dep, and resulting dictionary of u_dep_dict.
M_v = zeros(3, 9)
for i in range(3):
for j, ui in enumerate(u + ua):
M_v[i, j] = f_v[i].diff(ui)
M_v_i = M_v[:, :3]
M_v_d = M_v[:, 3:6]
M_v_aux = M_v[:, 6:]
M_v_i_aux = M_v_i.row_join(M_v_aux)
A_rs = - M_v_d.inv() * M_v_i_aux
u_dep = A_rs[:, :3] * Matrix(u[:3])
u_dep_dict = dict(zip(u[3:], u_dep))
# Active forces: F_O acting on point O; F_P acting on point P.
# Generalized active forces (unconstrained): Fr_u = F_point * pv_point.
F_O = m*g*A.z
F_P = Fx * A.x + Fy * A.y + Fz * A.z
Fr_u = Matrix([dot(F_O, pv_o) + dot(F_P, pv_p) for pv_o, pv_p in
zip(partial_v_O, partial_v_P)])
# Inertia force: R_star_O.
# Inertia of disc: I_C_O, where J is a inertia component about principal axis.
# Inertia torque: T_star_C.
# Generalized inertia forces (unconstrained): Fr_star_u.
R_star_O = -m*O.acc(N)
I_C_O = inertia(B, I, J, I)
T_star_C = -(dot(I_C_O, C.ang_acc_in(N)) \
+ cross(C.ang_vel_in(N), dot(I_C_O, C.ang_vel_in(N))))
Fr_star_u = Matrix([dot(R_star_O, pv) + dot(T_star_C, pav) for pv, pav in
zip(partial_v_O, partial_w_C)])
# Form nonholonomic Fr: Fr_c, and nonholonomic Fr_star: Fr_star_c.
# Also, nonholonomic Fr_star in steady turning condition: Fr_star_steady.
Fr_c = Fr_u[:3, :].col_join(Fr_u[6:, :]) + A_rs.T * Fr_u[3:6, :]
Fr_star_c = Fr_star_u[:3, :].col_join(Fr_star_u[6:, :])\
+ A_rs.T * Fr_star_u[3:6, :]
Fr_star_steady = Fr_star_c.subs(ud_zero).subs(u_dep_dict)\
.subs(steady_conditions).subs({q[3]: -r*cos(q[1])}).expand()
# Second, using KaneMethod in mechanics for fr, frstar and frstar_steady.
# Rigid Bodies: disc, with inertia I_C_O.
iner_tuple = (I_C_O, O)
disc = RigidBody('disc', O, C, m, iner_tuple)
bodyList = [disc]
# Generalized forces: Gravity: F_o; Auxiliary forces: F_p.
F_o = (O, F_O)
F_p = (P, F_P)
forceList = [F_o, F_p]
# KanesMethod.
kane = KanesMethod(
N, q_ind= q[:3], u_ind= u[:3], kd_eqs=kindiffs,
q_dependent=q[3:], configuration_constraints = f_c,
u_dependent=u[3:], velocity_constraints= f_v,
u_auxiliary=ua
)
# fr, frstar, frstar_steady and kdd(kinematic differential equations).
with warnings.catch_warnings():
warnings.filterwarnings("ignore", category=SymPyDeprecationWarning)
(fr, frstar)= kane.kanes_equations(forceList, bodyList)
frstar_steady = frstar.subs(ud_zero).subs(u_dep_dict).subs(steady_conditions)\
.subs({q[3]: -r*cos(q[1])}).expand()
kdd = kane.kindiffdict()
assert Matrix(Fr_c).expand() == fr.expand()
assert Matrix(Fr_star_c.subs(kdd)).expand() == frstar.expand()
assert (simplify(Matrix(Fr_star_steady).expand()) ==
simplify(frstar_steady.expand()))
def test_pinjoint_arbitrary_axis():
theta, omega = dynamicsymbols('theta_J, omega_J')
# When the bodies are attached though masscenters but axess are opposite.
N, A, P, C = _generate_body()
PinJoint('J', P, C, child_axis=-A.x)
assert -A.x.angle_between(N.x) == -pi
assert -A.x.express(N) == N.x
assert A.dcm(N) == Matrix([[-1, 0, 0], [0, -cos(theta), -sin(theta)],
[0, -sin(theta), cos(theta)]])
assert A.ang_vel_in(N) == omega * N.x
assert A.ang_vel_in(N).magnitude() == sqrt(omega**2)
assert C.masscenter.pos_from(P.masscenter) == 0
assert C.masscenter.pos_from(P.masscenter).express(N).simplify() == 0
assert C.masscenter.vel(N) == 0
# When axes are different and parent joint is at masscenter but child joint
# is at a unit vector from child masscenter.
N, A, P, C = _generate_body()
PinJoint('J', P, C, child_axis=A.y, child_joint_pos=A.x)
assert A.y.angle_between(N.x) == 0 # Axis are aligned
assert A.y.express(N) == N.x
assert A.dcm(N) == Matrix([[0, -cos(theta), -sin(theta)], [1, 0, 0],
[0, -sin(theta), cos(theta)]])
assert A.ang_vel_in(N) == omega * N.x
assert A.ang_vel_in(N).express(A) == omega * A.y
assert A.ang_vel_in(N).magnitude() == sqrt(omega**2)
angle = A.ang_vel_in(N).angle_between(A.y)
assert expand_mul(angle).xreplace({omega: 1}) == 0
assert C.masscenter.vel(N) == omega * A.z
assert C.masscenter.pos_from(P.masscenter) == -A.x
assert (C.masscenter.pos_from(
P.masscenter).express(N).simplify() == cos(theta) * N.y +
sin(theta) * N.z)
assert C.masscenter.vel(N).angle_between(A.x) == pi / 2
# Similar to previous case but wrt parent body
N, A, P, C = _generate_body()
PinJoint('J', P, C, parent_axis=N.y, parent_joint_pos=N.x)
assert N.y.angle_between(A.x) == 0 # Axis are aligned
assert N.y.express(A) == A.x
assert A.dcm(N) == Matrix([[0, 1, 0], [-cos(theta), 0,
sin(theta)],
[sin(theta), 0, cos(theta)]])
assert A.ang_vel_in(N) == omega * N.y
assert A.ang_vel_in(N).express(A) == omega * A.x
assert A.ang_vel_in(N).magnitude() == sqrt(omega**2)
angle = A.ang_vel_in(N).angle_between(A.x)
assert expand_mul(angle).xreplace({omega: 1}) == 0
assert C.masscenter.vel(N).simplify() == -omega * N.z
assert C.masscenter.pos_from(P.masscenter) == N.x
# Both joint pos id defined but different axes
N, A, P, C = _generate_body()
PinJoint('J',
P,
C,
parent_joint_pos=N.x,
child_joint_pos=A.x,
child_axis=A.x + A.y)
assert expand_mul(N.x.angle_between(A.x + A.y)) == 0 # Axis are aligned
assert (A.x + A.y).express(N).simplify() == sqrt(2) * N.x
assert A.dcm(N).simplify() == Matrix(
[[sqrt(2) / 2, -sqrt(2) * cos(theta) / 2, -sqrt(2) * sin(theta) / 2],
[sqrt(2) / 2,
sqrt(2) * cos(theta) / 2,
sqrt(2) * sin(theta) / 2], [0, -sin(theta),
cos(theta)]])
assert A.ang_vel_in(N) == omega * N.x
assert (
A.ang_vel_in(N).express(A).simplify() == (omega * A.x + omega * A.y) /
sqrt(2))
assert A.ang_vel_in(N).magnitude() == sqrt(omega**2)
angle = A.ang_vel_in(N).angle_between(A.x + A.y)
assert expand_mul(angle).xreplace({omega: 1}) == 0
assert C.masscenter.vel(N).simplify() == (omega * A.z) / sqrt(2)
assert C.masscenter.pos_from(P.masscenter) == N.x - A.x
assert (C.masscenter.pos_from(
P.masscenter).express(N).simplify() == (1 - sqrt(2) / 2) * N.x +
sqrt(2) * cos(theta) / 2 * N.y + sqrt(2) * sin(theta) / 2 * N.z)
assert (C.masscenter.vel(N).express(N).simplify() == -sqrt(2) * omega *
sin(theta) / 2 * N.y + sqrt(2) * omega * cos(theta) / 2 * N.z)
assert C.masscenter.vel(N).angle_between(A.x) == pi / 2
N, A, P, C = _generate_body()
PinJoint('J',
P,
C,
parent_joint_pos=N.x,
child_joint_pos=A.x,
child_axis=A.x + A.y - A.z)
assert expand_mul(N.x.angle_between(A.x + A.y - A.z)) == 0 # Axis aligned
assert (A.x + A.y - A.z).express(N).simplify() == sqrt(3) * N.x
assert A.dcm(N).simplify() == Matrix(
[[
sqrt(3) / 3, -sqrt(6) * sin(theta + pi / 4) / 3,
sqrt(6) * cos(theta + pi / 4) / 3
],
[
sqrt(3) / 3,
sqrt(6) * cos(theta + pi / 12) / 3,
sqrt(6) * sin(theta + pi / 12) / 3
],
[
-sqrt(3) / 3,
sqrt(6) * cos(theta + 5 * pi / 12) / 3,
sqrt(6) * sin(theta + 5 * pi / 12) / 3
]])
assert A.ang_vel_in(N) == omega * N.x
assert A.ang_vel_in(N).express(
A).simplify() == (omega * A.x + omega * A.y - omega * A.z) / sqrt(3)
assert A.ang_vel_in(N).magnitude() == sqrt(omega**2)
angle = A.ang_vel_in(N).angle_between(A.x + A.y - A.z)
assert expand_mul(angle).xreplace({omega: 1}) == 0
assert C.masscenter.vel(N).simplify() == (omega * A.y +
omega * A.z) / sqrt(3)
assert C.masscenter.pos_from(P.masscenter) == N.x - A.x
assert (C.masscenter.pos_from(
P.masscenter).express(N).simplify() == (1 - sqrt(3) / 3) * N.x +
sqrt(6) * sin(theta + pi / 4) / 3 * N.y -
sqrt(6) * cos(theta + pi / 4) / 3 * N.z)
assert (C.masscenter.vel(N).express(N).simplify() == sqrt(6) * omega *
cos(theta + pi / 4) / 3 * N.y +
sqrt(6) * omega * sin(theta + pi / 4) / 3 * N.z)
assert C.masscenter.vel(N).angle_between(A.x) == pi / 2
N, A, P, C = _generate_body()
m, n = symbols('m n')
PinJoint('J',
P,
C,
parent_joint_pos=m * N.x,
child_joint_pos=n * A.x,
child_axis=A.x + A.y - A.z,
parent_axis=N.x - N.y + N.z)
angle = (N.x - N.y + N.z).angle_between(A.x + A.y - A.z)
assert expand_mul(angle) == 0 # Axis are aligned
assert ((
A.x - A.y +
A.z).express(N).simplify() == (-4 * cos(theta) / 3 - 1 / 3) * N.x +
(1 / 3 - 4 * sin(theta + pi / 6) / 3) * N.y +
(4 * cos(theta + pi / 3) / 3 - 1 / 3) * N.z)
assert A.dcm(N).simplify() == Matrix(
[[
S(1) / 3 - 2 * cos(theta) / 3,
-2 * sin(theta + pi / 6) / 3 - S(1) / 3,
2 * cos(theta + pi / 3) / 3 + S(1) / 3
],
[
2 * cos(theta + pi / 3) / 3 + S(1) / 3,
2 * cos(theta) / 3 - S(1) / 3,
2 * sin(theta + pi / 6) / 3 + S(1) / 3
],
[
-2 * sin(theta + pi / 6) / 3 - S(1) / 3,
2 * cos(theta + pi / 3) / 3 + S(1) / 3,
2 * cos(theta) / 3 - S(1) / 3
]])
assert A.ang_vel_in(N) == (omega * N.x - omega * N.y +
omega * N.z) / sqrt(3)
assert A.ang_vel_in(N).express(
A).simplify() == (omega * A.x + omega * A.y - omega * A.z) / sqrt(3)
assert A.ang_vel_in(N).magnitude() == sqrt(omega**2)
angle = A.ang_vel_in(N).angle_between(A.x + A.y - A.z)
assert expand_mul(angle).xreplace({omega: 1}) == 0
assert (
C.masscenter.vel(N).simplify() == (m * omega * N.y + m * omega * N.z +
n * omega * A.y + n * omega * A.z) /
sqrt(3))
assert C.masscenter.pos_from(P.masscenter) == m * N.x - n * A.x
assert (C.masscenter.pos_from(
P.masscenter).express(N).simplify() == (m + n *
(2 * cos(theta) - 1) / 3) * N.x
+ n * (2 * sin(theta + pi / 6) + 1) / 3 * N.y - n *
(2 * cos(theta + pi / 3) + 1) / 3 * N.z)
assert (C.masscenter.vel(N).express(N).simplify() == -2 * n * omega *
sin(theta) / 3 * N.x +
(sqrt(3) * m + 2 * n * cos(theta + pi / 6)) * omega / 3 * N.y +
(sqrt(3) * m + 2 * n * sin(theta + pi / 3)) * omega / 3 * N.z)
assert expand_mul(C.masscenter.vel(N).angle_between(m * N.x -
n * A.x)) == pi / 2
def test_sub_qdot2():
# This test solves exercises 8.3 from Kane 1985 and defines
# all velocities in terms of q, qdot. We check that the generalized active
# forces are correctly computed if u terms are only defined in the
# kinematic differential equations.
#
# This functionality was added in PR 8948. Without qdot/u substitution, the
# KanesMethod constructor will fail during the constraint initialization as
# the B matrix will be poorly formed and inversion of the dependent part
# will fail.
g, m, Px, Py, Pz, R, t = symbols('g m Px Py Pz R t')
q = dynamicsymbols('q:5')
qd = dynamicsymbols('q:5', level=1)
u = dynamicsymbols('u:5')
## Define inertial, intermediate, and rigid body reference frames
A = ReferenceFrame('A')
B_prime = A.orientnew('B_prime', 'Axis', [q[0], A.z])
B = B_prime.orientnew('B', 'Axis', [pi / 2 - q[1], B_prime.x])
C = B.orientnew('C', 'Axis', [q[2], B.z])
## Define points of interest and their velocities
pO = Point('O')
pO.set_vel(A, 0)
# R is the point in plane H that comes into contact with disk C.
pR = pO.locatenew('R', q[3] * A.x + q[4] * A.y)
pR.set_vel(A, pR.pos_from(pO).diff(t, A))
pR.set_vel(B, 0)
# C^ is the point in disk C that comes into contact with plane H.
pC_hat = pR.locatenew('C^', 0)
pC_hat.set_vel(C, 0)
# C* is the point at the center of disk C.
pCs = pC_hat.locatenew('C*', R * B.y)
pCs.set_vel(C, 0)
pCs.set_vel(B, 0)
# calculate velocites of points C* and C^ in frame A
pCs.v2pt_theory(pR, A, B) # points C* and R are fixed in frame B
pC_hat.v2pt_theory(pCs, A, C) # points C* and C^ are fixed in frame C
## Define forces on each point of the system
R_C_hat = Px * A.x + Py * A.y + Pz * A.z
R_Cs = -m * g * A.z
forces = [(pC_hat, R_C_hat), (pCs, R_Cs)]
## Define kinematic differential equations
# let ui = omega_C_A & bi (i = 1, 2, 3)
# u4 = qd4, u5 = qd5
u_expr = [C.ang_vel_in(A) & uv for uv in B]
u_expr += qd[3:]
kde = [ui - e for ui, e in zip(u, u_expr)]
km1 = KanesMethod(A, q, u, kde)
with warns_deprecated_sympy():
fr1, _ = km1.kanes_equations(forces, [])
## Calculate generalized active forces if we impose the condition that the
# disk C is rolling without slipping
u_indep = u[:3]
u_dep = list(set(u) - set(u_indep))
vc = [pC_hat.vel(A) & uv for uv in [A.x, A.y]]
km2 = KanesMethod(A,
q,
u_indep,
kde,
u_dependent=u_dep,
velocity_constraints=vc)
with warns_deprecated_sympy():
fr2, _ = km2.kanes_equations(forces, [])
fr1_expected = Matrix([
-R * g * m * sin(q[1]),
-R * (Px * cos(q[0]) + Py * sin(q[0])) * tan(q[1]),
R * (Px * cos(q[0]) + Py * sin(q[0])), Px, Py
])
fr2_expected = Matrix([-R * g * m * sin(q[1]), 0, 0])
assert (trigsimp(fr1.expand()) == trigsimp(fr1_expected.expand()))
assert (trigsimp(fr2.expand()) == trigsimp(fr2_expected.expand()))
def __init__(self):
super(Quad, self).__init__()
# states
phi, theta, psi_, P, Q, R, x, y, z, U, V, W = mech.dynamicsymbols('phi, theta, psi_, P, Q, R, x, y, z, U, V, W')
self.x = sympy.Matrix([phi, theta, psi_, P, Q, R, x, y, z, U, V, W])
self.x0 = {
phi : 0,
theta : 0,
psi_ : 0,
P : 0,
Q : 0,
R : 0,
x : 0,
y : 0,
z : 0,
U : 0,
V : 0,
W : 0,
}
# variables
F_x, F_y, F_z, M_x, M_y, M_z = mech.dynamicsymbols('F_x, F_y, F_z, M_x, M_y, M_z')
self.v = sympy.Matrix([F_x, F_y, F_z, M_x, M_y, M_z])
# constants
self.c = sympy.Matrix([])
self.c0 = {
}
# parameters
J_x, J_y, J_z, m = sympy.symbols('J_x, J_y, J_z, m')
self.p = sympy.Matrix([J_x, J_y, J_z, m])
self.p0 = {
J_x : 1.0,
J_y : 1.0,
J_z : 1.0,
m : 1.0,
}
# inputs
self.u = sympy.Matrix([])
self.u0 = {
}
# outputs
self.y = sympy.Matrix([])
# equations
self.eqs = [
M_x - (- P - phi),
M_y - (- Q - theta),
M_z - (- R - psi_),
F_x - (- x),
F_y - (- y),
F_z - (- z),
(x).diff(self.t) - (U),
(y).diff(self.t) - (V),
(z).diff(self.t) - (W),
- m * V * (R).diff(self.t) + m * W * (Q).diff(self.t) + m * (U).diff(self.t) - (F_x),
m * U * (R).diff(self.t) - m * W * (P).diff(self.t) + m * (V).diff(self.t) - (F_y),
- m * U * (Q).diff(self.t) + m * V * (P).diff(self.t) + m * (W).diff(self.t) - (F_z),
(phi).diff(self.t) - (P + Q * sin(phi) * tan(theta) + R * cos(phi) * tan(theta)),
(theta).diff(self.t) - (Q * cos(phi) - R * sin(phi)),
cos(theta) * (psi_).diff(self.t) - (Q * sin(phi) + R * cos(phi)),
(P).diff(self.t) - (M_x),
(Q).diff(self.t) - (M_y),
(R).diff(self.t) - (M_z),
]
self.compute_fg()
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 test_nonminimal_pendulum():
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)
# 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()
# Check solution
lam1 = LM.lam_vec[0, 0]
eom_sol = Matrix([[m*Derivative(q1, t, t) - 9.8*m + 2*lam1*q1],
[m*Derivative(q2, t, t) + 2*lam1*q2]])
assert LM.eom == eom_sol
# Check multiplier solution
lam_sol = Matrix([(19.6*q1 + 2*q1d**2 + 2*q2d**2)/(4*q1**2/m + 4*q2**2/m)])
assert LM.solve_multipliers(sol_type='Matrix') == lam_sol
def test_n_link_pendulum_on_cart_inputs():
l0, m0 = symbols("l0 m0")
m1 = symbols("m1")
g = symbols("g")
q0, q1, F, T1 = dynamicsymbols("q0 q1 F T1")
u0, u1 = dynamicsymbols("u0 u1")
kane1 = models.n_link_pendulum_on_cart(1)
massmatrix1 = Matrix([[m0 + m1, -l0*m1*cos(q1)],
[-l0*m1*cos(q1), l0**2*m1]])
forcing1 = Matrix([[-l0*m1*u1**2*sin(q1) + F], [g*l0*m1*sin(q1)]])
assert simplify(massmatrix1 - kane1.mass_matrix) == zeros(2)
assert simplify(forcing1 - kane1.forcing) == Matrix([0, 0])
kane2 = models.n_link_pendulum_on_cart(1, False)
massmatrix2 = Matrix([[m0 + m1, -l0*m1*cos(q1)],
[-l0*m1*cos(q1), l0**2*m1]])
forcing2 = Matrix([[-l0*m1*u1**2*sin(q1)], [g*l0*m1*sin(q1)]])
assert simplify(massmatrix2 - kane2.mass_matrix) == zeros(2)
assert simplify(forcing2 - kane2.forcing) == Matrix([0, 0])
kane3 = models.n_link_pendulum_on_cart(1, False, True)
massmatrix3 = Matrix([[m0 + m1, -l0*m1*cos(q1)],
[-l0*m1*cos(q1), l0**2*m1]])
forcing3 = Matrix([[-l0*m1*u1**2*sin(q1)], [g*l0*m1*sin(q1) + T1]])
assert simplify(massmatrix3 - kane3.mass_matrix) == zeros(2)
assert simplify(forcing3 - kane3.forcing) == Matrix([0, 0])
kane4 = models.n_link_pendulum_on_cart(1, True, False)
massmatrix4 = Matrix([[m0 + m1, -l0*m1*cos(q1)],
[-l0*m1*cos(q1), l0**2*m1]])
forcing4 = Matrix([[-l0*m1*u1**2*sin(q1) + F], [g*l0*m1*sin(q1)]])
assert simplify(massmatrix4 - kane4.mass_matrix) == zeros(2)
assert simplify(forcing4 - kane4.forcing) == Matrix([0, 0])
def setup(self):
# System state variables
q = me.dynamicsymbols('q')
qd = me.dynamicsymbols('q', 1)
# Other system variables
m, k, b = symbols('m k b')
# Set up the reference frame
N = me.ReferenceFrame('N')
# Set up the points and particles
P = me.Point('P')
P.set_vel(N, qd * N.x)
Pa = me.Particle('Pa', P, m)
# Define the potential energy and create the Lagrangian
Pa.potential_energy = k * q**2 / 2.0
L = me.Lagrangian(N, Pa)
# Create the list of forces acting on the system
fl = [(P, -b * qd * N.x)]
# Create an instance of Lagranges Method
self.l = me.LagrangesMethod(L, [q], forcelist=fl, frame=N)
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(FL, BL)
MM = KM.mass_matrix
forcing = KM.forcing
rhs = MM.inv() * forcing
assert expand(rhs[0]) == expand(-(q * k + u * c) / m)
assert (KM.linearize(A_and_B=True, new_method=True)[0] ==
Matrix([[0, 1], [-k/m, -c/m]]))
# Ensure that the old linearizer still works and that the new linearizer
# gives the same results. The old linearizer is deprecated and should be
# removed in >= 0.7.7.
M_old = KM.mass_matrix_full
# The old linearizer raises a deprecation warning, so catch it here so
# it doesn't cause py.test to fail.
with warnings.catch_warnings():
warnings.filterwarnings("ignore", category=SymPyDeprecationWarning)
F_A_old, F_B_old, r_old = KM.linearize()
M_new, F_A_new, F_B_new, r_new = KM.linearize(new_method=True)
assert simplify(M_new.inv() * F_A_new - M_old.inv() * F_A_old) == zeros(2)
def test_point_funcs():
q, q2 = dynamicsymbols('q q2')
qd, q2d = dynamicsymbols('q q2', 1)
qdd, q2dd = dynamicsymbols('q q2', 2)
N = ReferenceFrame('N')
B = ReferenceFrame('B')
B.set_ang_vel(N, 5 * B.y)
O = Point('O')
P = O.locatenew('P', q * B.x)
assert P.pos_from(O) == q * B.x
P.set_vel(B, qd * B.x + q2d * B.y)
assert P.vel(B) == qd * B.x + q2d * B.y
O.set_vel(N, 0)
assert O.vel(N) == 0
assert P.a1pt_theory(O, N, B) == ((-25 * q + qdd) * B.x + (q2dd) * B.y +
(-10 * qd) * B.z)
B = N.orientnew('B', 'Axis', [q, N.z])
O = Point('O')
P = O.locatenew('P', 10 * B.x)
O.set_vel(N, 5 * N.x)
assert O.vel(N) == 5 * N.x
assert P.a2pt_theory(O, N, B) == (-10 * qd**2) * B.x + (10 * qdd) * B.y
B.set_ang_vel(N, 5 * B.y)
O = Point('O')
P = O.locatenew('P', q * B.x)
P.set_vel(B, qd * B.x + q2d * B.y)
O.set_vel(N, 0)
assert P.v1pt_theory(O, N, B) == qd * B.x + q2d * B.y - 5 * q * B.z
def test_partial_velocity():
q1, q2, q3, u1, u2, u3 = dynamicsymbols("q1 q2 q3 u1 u2 u3")
u4, u5 = dynamicsymbols("u4, u5")
r = symbols("r")
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])
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)
vel_list = [Dmc.vel(N), C.vel(N), R.ang_vel_in(N)]
u_list = [u1, u2, u3, u4, u5]
assert partial_velocity(vel_list, u_list) == [
[-r * L.y, 0, L.x],
[r * L.x, 0, L.y],
[0, 0, L.z],
[L.x, L.x, 0],
[cos(q2) * L.y - sin(q2) * L.z, cos(q2) * L.y - sin(q2) * L.z, 0],
]
def __init__(self, coordinate_name, *args, **kwargs):
super(RevoluteJoint, self).__init__(*args, **kwargs)
# TODO where should these be stored?
# TODO this would manage creating an extra speed for a quaternion.
# TODO ensure coordinate names are not getting overwritten.
self._rotation = dynamicsymbols(coordinate_name)
self._rotationdot = dynamicsymbols(coordinate_name, 1)
self._rotspeed = dynamicsymbols('%s_u' % coordinate_name)
self._rotspeeddot = dynamicsymbols('%s_u' % coordinate_name, 1)