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_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_linearize_pendulum_kane_nonminimal():
# Create generalized coordinates and speeds for this non-minimal realization
# q1, q2 = N.x and N.y coordinates of pendulum
# u1, u2 = N.x and N.y velocities of pendulum
q1, q2 = dynamicsymbols('q1:3')
q1d, q2d = dynamicsymbols('q1:3', level=1)
u1, u2 = dynamicsymbols('u1:3')
u1d, u2d = dynamicsymbols('u1:3', level=1)
L, m, t = symbols('L, m, t')
g = 9.8
# Compose world frame
N = ReferenceFrame('N')
pN = Point('N*')
pN.set_vel(N, 0)
# A.x is along the pendulum
theta1 = atan(q2/q1)
A = N.orientnew('A', 'axis', [theta1, N.z])
# Locate the pendulum mass
P = pN.locatenew('P1', q1*N.x + q2*N.y)
pP = Particle('pP', P, m)
# Calculate the kinematic differential equations
kde = Matrix([q1d - u1,
q2d - u2])
dq_dict = solve(kde, [q1d, q2d])
# Set velocity of point P
P.set_vel(N, P.pos_from(pN).dt(N).subs(dq_dict))
# Configuration constraint is length of pendulum
f_c = Matrix([P.pos_from(pN).magnitude() - L])
# Velocity constraint is that the velocity in the A.x direction is
# always zero (the pendulum is never getting longer).
f_v = Matrix([P.vel(N).express(A).dot(A.x)])
f_v.simplify()
# Acceleration constraints is the time derivative of the velocity constraint
f_a = f_v.diff(t)
f_a.simplify()
# Input the force resultant at P
R = m*g*N.x
# Derive the equations of motion using the KanesMethod class.
KM = KanesMethod(N, q_ind=[q2], u_ind=[u2], q_dependent=[q1],
u_dependent=[u1], configuration_constraints=f_c,
velocity_constraints=f_v, acceleration_constraints=f_a, kd_eqs=kde)
with warns_deprecated_sympy():
(fr, frstar) = KM.kanes_equations([(P, R)], [pP])
# Set the operating point to be straight down, and non-moving
q_op = {q1: L, q2: 0}
u_op = {u1: 0, u2: 0}
ud_op = {u1d: 0, u2d: 0}
A, B, inp_vec = KM.linearize(op_point=[q_op, u_op, ud_op], A_and_B=True,
simplify=True)
assert A.expand() == Matrix([[0, 1], [-9.8/L, 0]])
assert B == Matrix([])
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([[1, 0, 0, 0, 0], [0, 1, 0, 0, 0],
[0, 0, 1, 0, -x / m], [0, 0, 0, 1, -y / m],
[0, 0, 0, 0, l**2 / m]])
comb_implicit_rhs = Matrix([u, v, 0, 0, u**2 + v**2 - g * y])
kin_explicit_rhs = Matrix([u, v])
comb_explicit_rhs = comb_implicit_mat.LUsolve(comb_implicit_rhs)
# Set up a body and load to pass into the system
theta = atan(x / y)
N = ReferenceFrame('N')
A = N.orientnew('A', 'Axis', [theta, N.z])
O = Point('O')
P = O.locatenew('P', l * A.x)
Pa = Particle('Pa', P, m)
bodies = [Pa]
loads = [(P, g * m * N.x)]
# Set up some output equations to be given to SymbolicSystem
# Change to make these fit the pendulum
PE = symbols("PE")
out_eqns = {PE: m * g * (l + y)}
dyn_implicit_rhs = Matrix([0, 0, u**2 + v**2 - g*y])
comb_implicit_mat = Matrix([[1, 0, 0, 0, 0],
[0, 1, 0, 0, 0],
[0, 0, 1, 0, -x/m],
[0, 0, 0, 1, -y/m],
[0, 0, 0, 0, l**2/m]])
comb_implicit_rhs = Matrix([u, v, 0, 0, u**2 + v**2 - g*y])
kin_explicit_rhs = Matrix([u, v])
comb_explicit_rhs = comb_implicit_mat.LUsolve(comb_implicit_rhs)
# Set up a body and load to pass into the system
theta = atan(x / y)
N = ReferenceFrame('N')
A = N.orientnew('A', 'Axis', [theta, N.z])
O = Point('O')
P = O.locatenew('P', l * A.x)
Pa = Particle('Pa', P, m)
bodies = [Pa]
loads = [(P, g * m * N.x)]
# Set up some output equations to be given to SymbolicSystem
# Change to make these fit the pendulum
PE = symbols("PE")
out_eqns = {PE: m*g*(l+y)}
def test_linearize_pendulum_kane_nonminimal():
# Create generalized coordinates and speeds for this non-minimal realization
# q1, q2 = N.x and N.y coordinates of pendulum
# u1, u2 = N.x and N.y velocities of pendulum
q1, q2 = dynamicsymbols('q1:3')
q1d, q2d = dynamicsymbols('q1:3', level=1)
u1, u2 = dynamicsymbols('u1:3')
u1d, u2d = dynamicsymbols('u1:3', level=1)
L, m, t = symbols('L, m, t')
g = 9.8
# Compose world frame
N = ReferenceFrame('N')
pN = Point('N*')
pN.set_vel(N, 0)
# A.x is along the pendulum
theta1 = atan(q2/q1)
A = N.orientnew('A', 'axis', [theta1, N.z])
# Locate the pendulum mass
P = pN.locatenew('P1', q1*N.x + q2*N.y)
pP = Particle('pP', P, m)
# Calculate the kinematic differential equations
kde = Matrix([q1d - u1,
q2d - u2])
dq_dict = solve(kde, [q1d, q2d])
# Set velocity of point P
P.set_vel(N, P.pos_from(pN).dt(N).subs(dq_dict))
# Configuration constraint is length of pendulum
f_c = Matrix([P.pos_from(pN).magnitude() - L])
# Velocity constraint is that the velocity in the A.x direction is
# always zero (the pendulum is never getting longer).
f_v = Matrix([P.vel(N).express(A).dot(A.x)])
f_v.simplify()
# Acceleration constraints is the time derivative of the velocity constraint
f_a = f_v.diff(t)
f_a.simplify()
# Input the force resultant at P
R = m*g*N.x
# Derive the equations of motion using the KanesMethod class.
KM = KanesMethod(N, q_ind=[q2], u_ind=[u2], q_dependent=[q1],
u_dependent=[u1], configuration_constraints=f_c,
velocity_constraints=f_v, acceleration_constraints=f_a, kd_eqs=kde)
with warnings.catch_warnings():
warnings.filterwarnings("ignore", category=SymPyDeprecationWarning)
(fr, frstar) = KM.kanes_equations([(P, R)], [pP])
# Set the operating point to be straight down, and non-moving
q_op = {q1: L, q2: 0}
u_op = {u1: 0, u2: 0}
ud_op = {u1d: 0, u2d: 0}
A, B, inp_vec = KM.linearize(op_point=[q_op, u_op, ud_op], A_and_B=True,
new_method=True, simplify=True)
assert A.expand() == Matrix([[0, 1], [-9.8/L, 0]])
assert B == Matrix([])
