ai-content-maker/.venv/Lib/site-packages/sympy/physics/mechanics/tests/test_kane.py

533 lines
20 KiB
Python

from sympy import solve
from sympy.core.backend import (cos, expand, Matrix, sin, symbols, tan, sqrt, S,
zeros, eye)
from sympy.simplify.simplify import simplify
from sympy.physics.mechanics import (dynamicsymbols, ReferenceFrame, Point,
RigidBody, KanesMethod, inertia, Particle,
dot)
from sympy.testing.pytest import raises
from sympy.core.backend import USE_SYMENGINE
def test_invalid_coordinates():
# Simple pendulum, but use symbols instead of dynamicsymbols
l, m, g = symbols('l m g')
q, u = symbols('q u') # Generalized coordinate
kd = [q.diff(dynamicsymbols._t) - u]
N, O = ReferenceFrame('N'), Point('O')
O.set_vel(N, 0)
P = Particle('P', Point('P'), m)
P.point.set_pos(O, l * (sin(q) * N.x - cos(q) * N.y))
F = (P.point, -m * g * N.y)
raises(ValueError, lambda: KanesMethod(N, [q], [u], kd, bodies=[P],
forcelist=[F]))
def test_one_dof():
# This is for a 1 dof spring-mass-damper case.
# It is described in more detail in the KanesMethod docstring.
q, u = dynamicsymbols('q u')
qd, ud = dynamicsymbols('q u', 1)
m, c, k = symbols('m c k')
N = ReferenceFrame('N')
P = Point('P')
P.set_vel(N, u * N.x)
kd = [qd - u]
FL = [(P, (-k * q - c * u) * N.x)]
pa = Particle('pa', P, m)
BL = [pa]
KM = KanesMethod(N, [q], [u], kd)
KM.kanes_equations(BL, FL)
assert KM.bodies == BL
assert KM.loads == FL
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_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)
# Note we multiply the kinematic equation by an arbitrary factor
# to test the implicit vs explicit kinematics attribute
kd = [q1d/2 - u1/2, 2*q2d - 2*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(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)
# Check that the explicit form is the default and kinematic mass matrix is identity
assert KM.explicit_kinematics
assert KM.mass_matrix_kin == eye(2)
# Check that for the implicit form the mass matrix is not identity
KM.explicit_kinematics = False
assert KM.mass_matrix_kin == Matrix([[S(1)/2, 0], [0, 2]])
# Check that whether using implicit or explicit kinematics the RHS
# equations are consistent with the matrix form
for explicit_kinematics in [False, True]:
KM.explicit_kinematics = explicit_kinematics
assert simplify(KM.rhs() -
KM.mass_matrix_full.LUsolve(KM.forcing_full)) == zeros(4, 1)
# Make sure an error is raised if nonlinear kinematic differential
# equations are supplied.
kd = [q1d - u1**2, sin(q2d) - cos(u2)]
raises(ValueError, lambda: KanesMethod(N, q_ind=[q1, q2],
u_ind=[u1, u2], kd_eqs=kd))
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', P, m)
BL = [pa]
KM = KanesMethod(N, [q], [u], kd)
KM.kanes_equations(BL, FL)
MM = KM.mass_matrix
forcing = KM.forcing
rhs = MM.inv() * forcing
rhs.simplify()
assert expand(rhs[0]) == expand(-g / l * sin(q))
assert simplify(KM.rhs() -
KM.mass_matrix_full.LUsolve(KM.forcing_full)) == zeros(2, 1)
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)
KM.kanes_equations(BodyList, ForceList)
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 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)
(fr, frstar) = KM.kanes_equations(BodyList, ForceList)
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])
(fr2, frstar2) = KM2.kanes_equations(BodyList, ForceList)
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_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)
(fr, frstar) = km.kanes_equations(bodyList, forceList)
mm = km.mass_matrix_full
assert mm[3, 3] == Iz
def test_input_format():
# 1 dof problem from test_one_dof
q, u = dynamicsymbols('q u')
qd, ud = dynamicsymbols('q u', 1)
m, c, k = symbols('m c k')
N = ReferenceFrame('N')
P = Point('P')
P.set_vel(N, u * N.x)
kd = [qd - u]
FL = [(P, (-k * q - c * u) * N.x)]
pa = Particle('pa', P, m)
BL = [pa]
KM = KanesMethod(N, [q], [u], kd)
# test for input format kane.kanes_equations((body1, body2, particle1))
assert KM.kanes_equations(BL)[0] == Matrix([0])
# test for input format kane.kanes_equations(bodies=(body1, body 2), loads=(load1,load2))
assert KM.kanes_equations(bodies=BL, loads=None)[0] == Matrix([0])
# test for input format kane.kanes_equations(bodies=(body1, body 2), loads=None)
assert KM.kanes_equations(BL, loads=None)[0] == Matrix([0])
# test for input format kane.kanes_equations(bodies=(body1, body 2))
assert KM.kanes_equations(BL)[0] == Matrix([0])
# test for input format kane.kanes_equations(bodies=(body1, body2), loads=[])
assert KM.kanes_equations(BL, [])[0] == Matrix([0])
# test for error raised when a wrong force list (in this case a string) is provided
raises(ValueError, lambda: KM._form_fr('bad input'))
# 1 dof problem from test_one_dof with FL & BL in instance
KM = KanesMethod(N, [q], [u], kd, bodies=BL, forcelist=FL)
assert KM.kanes_equations()[0] == Matrix([-c*u - k*q])
# 2 dof problem from test_two_dof
q1, q2, u1, u2 = dynamicsymbols('q1 q2 u1 u2')
q1d, q2d, u1d, u2d = dynamicsymbols('q1 q2 u1 u2', 1)
m, c1, c2, k1, k2 = symbols('m c1 c2 k1 k2')
N = ReferenceFrame('N')
P1 = Point('P1')
P2 = Point('P2')
P1.set_vel(N, u1 * N.x)
P2.set_vel(N, (u1 + u2) * N.x)
kd = [q1d - u1, q2d - u2]
FL = ((P1, (-k1 * q1 - c1 * u1 + k2 * q2 + c2 * u2) * N.x), (P2, (-k2 *
q2 - c2 * u2) * N.x))
pa1 = Particle('pa1', P1, m)
pa2 = Particle('pa2', P2, m)
BL = (pa1, pa2)
KM = KanesMethod(N, q_ind=[q1, q2], u_ind=[u1, u2], kd_eqs=kd)
# test for input format
# kane.kanes_equations((body1, body2), (load1, load2))
KM.kanes_equations(BL, FL)
MM = KM.mass_matrix
forcing = KM.forcing
rhs = MM.inv() * forcing
assert expand(rhs[0]) == expand((-k1 * q1 - c1 * u1 + k2 * q2 + c2 * u2)/m)
assert expand(rhs[1]) == expand((k1 * q1 + c1 * u1 - 2 * k2 * q2 - 2 *
c2 * u2) / m)
def test_implicit_kinematics():
# Test that implicit kinematics can handle complicated
# equations that explicit form struggles with
# See https://github.com/sympy/sympy/issues/22626
# Inertial frame
NED = ReferenceFrame('NED')
NED_o = Point('NED_o')
NED_o.set_vel(NED, 0)
# body frame
q_att = dynamicsymbols('lambda_0:4', real=True)
B = NED.orientnew('B', 'Quaternion', q_att)
# Generalized coordinates
q_pos = dynamicsymbols('B_x:z')
B_cm = NED_o.locatenew('B_cm', q_pos[0]*B.x + q_pos[1]*B.y + q_pos[2]*B.z)
q_ind = q_att[1:] + q_pos
q_dep = [q_att[0]]
kinematic_eqs = []
# Generalized velocities
B_ang_vel = B.ang_vel_in(NED)
P, Q, R = dynamicsymbols('P Q R')
B.set_ang_vel(NED, P*B.x + Q*B.y + R*B.z)
B_ang_vel_kd = (B.ang_vel_in(NED) - B_ang_vel).simplify()
# Equating the two gives us the kinematic equation
kinematic_eqs += [
B_ang_vel_kd & B.x,
B_ang_vel_kd & B.y,
B_ang_vel_kd & B.z
]
B_cm_vel = B_cm.vel(NED)
U, V, W = dynamicsymbols('U V W')
B_cm.set_vel(NED, U*B.x + V*B.y + W*B.z)
# Compute the velocity of the point using the two methods
B_ref_vel_kd = (B_cm.vel(NED) - B_cm_vel)
# taking dot product with unit vectors to get kinematic equations
# relating body coordinates and velocities
# Note, there is a choice to dot with NED.xyz here. That makes
# the implicit form have some bigger terms but is still fine, the
# explicit form still struggles though
kinematic_eqs += [
B_ref_vel_kd & B.x,
B_ref_vel_kd & B.y,
B_ref_vel_kd & B.z,
]
u_ind = [U, V, W, P, Q, R]
# constraints
q_att_vec = Matrix(q_att)
config_cons = [(q_att_vec.T*q_att_vec)[0] - 1] #unit norm
kinematic_eqs = kinematic_eqs + [(q_att_vec.T * q_att_vec.diff())[0]]
try:
KM = KanesMethod(NED, q_ind, u_ind,
q_dependent= q_dep,
kd_eqs = kinematic_eqs,
configuration_constraints = config_cons,
velocity_constraints= [],
u_dependent= [], #no dependent speeds
u_auxiliary = [], # No auxiliary speeds
explicit_kinematics = False # implicit kinematics
)
except Exception as e:
# symengine struggles with these kinematic equations
if USE_SYMENGINE and 'Matrix is rank deficient' in str(e):
return
else:
raise e
# mass and inertia dyadic relative to CM
M_B = symbols('M_B')
J_B = inertia(B, *[S(f'J_B_{ax}')*(1 if ax[0] == ax[1] else -1)
for ax in ['xx', 'yy', 'zz', 'xy', 'yz', 'xz']])
J_B = J_B.subs({S('J_B_xy'): 0, S('J_B_yz'): 0})
RB = RigidBody('RB', B_cm, B, M_B, (J_B, B_cm))
rigid_bodies = [RB]
# Forces
force_list = [
#gravity pointing down
(RB.masscenter, RB.mass*S('g')*NED.z),
#generic forces and torques in body frame(inputs)
(RB.frame, dynamicsymbols('T_z')*B.z),
(RB.masscenter, dynamicsymbols('F_z')*B.z)
]
KM.kanes_equations(rigid_bodies, force_list)
# Expecting implicit form to be less than 5% of the flops
n_ops_implicit = sum(
[x.count_ops() for x in KM.forcing_full] +
[x.count_ops() for x in KM.mass_matrix_full]
)
# Save implicit kinematic matrices to use later
mass_matrix_kin_implicit = KM.mass_matrix_kin
forcing_kin_implicit = KM.forcing_kin
KM.explicit_kinematics = True
n_ops_explicit = sum(
[x.count_ops() for x in KM.forcing_full] +
[x.count_ops() for x in KM.mass_matrix_full]
)
forcing_kin_explicit = KM.forcing_kin
assert n_ops_implicit / n_ops_explicit < .05
# Ideally we would check that implicit and explicit equations give the same result as done in test_one_dof
# But the whole raison-d'etre of the implicit equations is to deal with problems such
# as this one where the explicit form is too complicated to handle, especially the angular part
# (i.e. tests would be too slow)
# Instead, we check that the kinematic equations are correct using more fundamental tests:
#
# (1) that we recover the kinematic equations we have provided
assert (mass_matrix_kin_implicit * KM.q.diff() - forcing_kin_implicit) == Matrix(kinematic_eqs)
# (2) that rate of quaternions matches what 'textbook' solutions give
# Note that we just use the explicit kinematics for the linear velocities
# as they are not as complicated as the angular ones
qdot_candidate = forcing_kin_explicit
quat_dot_textbook = Matrix([
[0, -P, -Q, -R],
[P, 0, R, -Q],
[Q, -R, 0, P],
[R, Q, -P, 0],
]) * q_att_vec / 2
# Again, if we don't use this "textbook" solution
# sympy will struggle to deal with the terms related to quaternion rates
# due to the number of operations involved
qdot_candidate[-1] = quat_dot_textbook[0] # lambda_0, note the [-1] as sympy's Kane puts the dependent coordinate last
qdot_candidate[0] = quat_dot_textbook[1] # lambda_1
qdot_candidate[1] = quat_dot_textbook[2] # lambda_2
qdot_candidate[2] = quat_dot_textbook[3] # lambda_3
# sub the config constraint in the candidate solution and compare to the implicit rhs
lambda_0_sol = solve(config_cons[0], q_att_vec[0])[1]
lhs_candidate = simplify(mass_matrix_kin_implicit * qdot_candidate).subs({q_att_vec[0]: lambda_0_sol})
assert lhs_candidate == forcing_kin_implicit