N = ReferenceFrame('N')
def eval_vec(v):
vs = v.subs(ab_val)
return sum(dot(vs, n).evalf() * n for n in N)
# For each part A, B, C, D, define an origin for that part such that the
# centers of mass of each part of the component have positive N.x, N.y,
# and N.z values.
## FIND CENTER OF MASS OF A
vol_A_1 = pi * a**2 * b / 4
vol_A_2 = pi * a**2 * a / 4 / 2
vol_A = vol_A_1 + vol_A_2
pA_O = Point('A_O')
pAs_1 = pA_O.locatenew(
'A*_1', (b / 2 * N.z + centroid_sector.subs(theta_pi_4) * sin(pi / 4) *
(N.x + N.y)))
pAs_2 = pA_O.locatenew('A*_2', (b * N.z + N.z * integrate(
(theta * R**2 * (z)).subs(R_theta_val),
(z, 0, a)) / vol_A_2 + N.x * integrate(
(theta * R**2 * cos(theta) * centroid_sector).subs(R_theta_val),
(z, 0, a)) / vol_A_2 + N.y * integrate(
(2 * R**3 / 3 * 4 * a / pi * sin(theta)**2).subs(R, a),
(theta, 0, pi / 4)) / vol_A_2))
pAs = pA_O.locatenew(
'A*', ((pAs_1.pos_from(pA_O) * vol_A_1 + pAs_2.pos_from(pA_O) * vol_A_2) /
vol_A))
print('A* = {0}'.format(pAs.pos_from(pA_O)))
print('A* = {0}'.format(eval_vec(pAs.pos_from(pA_O))))
q1, q2 = dynamicsymbols('q1 q2')
q1d, q2d = dynamicsymbols('q1 q2', 1)
theta, r, R, m = symbols('θ r R m', real=True, positive=True)
theta_val = pi / 2
#q2d_val = -(2*R + r)/(2*r)*q1d
q2d_val = (-(R + r * cos(theta)) / r * q1d)
N = ReferenceFrame('N')
#B = N.orientnew('B', 'body', [q1, theta, q2], 'zxz')
F1 = N.orientnew('F1', 'axis', [q1, N.z])
F2 = F1.orientnew('F2', 'axis', [theta, F1.x])
B = F2.orientnew('B', 'axis', [q2, F2.z])
# velocity of the disk at the point of contact with the ground is not moving
# since the disk rolls without slipping.
pA = Point('pA') # ball bearing A
pB = pA.locatenew('pB', -R * F1.y) # ball bearing B
pA.set_vel(N, 0)
pA.set_vel(F1, 0)
pB.set_vel(F1, 0)
pB.set_vel(B, 0)
pB.v2pt_theory(pA, N, F1)
#pC.v2pt_theory(pB, N, B)
#print('\nvelocity of point C in N, v_C_N, at q1 = 0 = ')
#print(pC.vel(N).express(N).subs(q2d, q2d_val))
Ixx = m * r**2 / 4
Iyy = m * r**2 / 4
q0d, q1d, q2d = dynamicsymbols('q0 q1 q2', level=1)
u1, u2, u3 = dynamicsymbols('u1 u2 u3')
LA, LB, LP = symbols('LA LB LP')
p1, p2, p3 = symbols('p1 p2 p3')
E = ReferenceFrame('E')
# A.x of Rigid Body A is fixed in Reference Frame E and is rotated by q0.
A = E.orientnew('A', 'Axis', [q0, E.x])
# B.y of Rigid Body B is fixed in Reference Frame A and is rotated by q1.
B = A.orientnew('B', 'Axis', [q1, A.y])
# Reference Frame C has no rotation relative to Reference Frame B.
C = B.orientnew('C', 'Axis', [0, B.x])
# Reference Frame D has no rotation relative to Reference Frame C.
D = C.orientnew('D', 'Axis', [0, C.x])
pO = Point('O')
# The vector from Point O to Point A*, the center of mass of A, is LA * A.z.
pAs = pO.locatenew('A*', LA * A.z)
# The vector from Point O to Point P, which lies on the axis where
# B rotates about A, is LP * A.z.
pP = pO.locatenew('P', LP * A.z)
# The vector from Point P to Point B*, the center of mass of B, is LB * B.z.
pBs = pP.locatenew('B*', LB * B.z)
# The vector from Point B* to Point C*, the center of mass of C, is q2 * B.z.
pCs = pBs.locatenew('C*', q2 * B.z)
# The vector from Point C* to Point D*, the center of mass of D,
# is p1 * B.x + p2 * B.y + p3 * B.z.
pDs = pCs.locatenew('D*', p1 * B.x + p2 * B.y + p3 * B.z)
# Define generalized speeds as:
# u1 = q0d
from __future__ import division
from sympy import collect, expand, expand_trig, symbols, trigsimp, sin, cos, S
from sympy.physics.mechanics import ReferenceFrame, RigidBody, Point
from sympy.physics.mechanics import dot, dynamicsymbols, inertia, msprint
q1, q2, q3 = dynamicsymbols('q1, q2 q3')
#omega1, omega2, omega3 = dynamicsymbols('ω1 ω2 ω3')
q1d, q2d = dynamicsymbols('q1, q2', level=1)
m, I11, I22, I33 = symbols('m I11 I22 I33', real=True, positive=True)
# reference frames
A = ReferenceFrame('A')
B = A.orientnew('B', 'body', [q1, q2, q3], 'xyz')
# points B*, O
pB_star = Point('B*')
pB_star.set_vel(A, 0)
# rigidbody B
I_B_Bs = inertia(B, I11, I22, I33)
rbB = RigidBody('rbB', pB_star, B, m, (I_B_Bs, pB_star))
# kinetic energy
K = rbB.kinetic_energy(A) # velocity of point B* is zero
print('K_ω = {0}'.format(msprint(K)))
print('\nSince I11, I22, I33 are the central principal moments of inertia')
print('let I_min = I11, I_max = I33')
I_min = I11
I_max = I33
H = rbB.angular_momentum(pB_star, A)
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
}
# 2a
q1, q2 = dynamicsymbols('q1 q2')
q1d, q2d = dynamicsymbols('q1 q2', 1)
r, R, m, g = symbols('r R m g', real=True, positive=True)
theta = pi / 3
q2d_val = (-(R + r * cos(theta)) / r * q1d)
vals = {q2d: q2d_val}
N = ReferenceFrame('N')
F1 = N.orientnew('F1', 'axis', [q1, N.z])
F2 = F1.orientnew('F2', 'axis', [theta, F1.x])
B = F2.orientnew('B', 'axis', [q2, F2.z])
# bearing A
pA = Point('A')
pA.set_vel(N, 0)
pA.set_vel(F1, 0)
# bearing B, center of mass of disc
pB = pA.locatenew('pB', -R * F1.y)
pB.set_vel(B, 0)
pB.v2pt_theory(pA, N, F1)
print('v_B_N = {}'.format(msprint(pB.vel(N))))
Ixx = m * r**2 / 4
Iyy = m * r**2 / 4
Izz = m * r**2 / 2
I_disc = inertia(F2, Ixx, Iyy, Izz, 0, 0, 0)
rb_disc = RigidBody('disc', pB, B, m, (I_disc, pB))
H = rb_disc.angular_momentum(pB, N).subs(vals).express(F2).simplify()
from sympy.physics.mechanics import cross, dot, dynamicsymbols
from util import msprint, subs, partial_velocities
from util import generalized_active_forces, potential_energy
q1, q2, q3, q4 = dynamicsymbols('q1:5')
q1d, q2d, q3d, q4d = dynamicsymbols('q1:5', level=1)
u1, u2, u3, u4 = dynamicsymbols('u1:5')
m, M, G, R = symbols('m M G R')
I1, I2, I3 = symbols('I1:4')
# reference frames
A = ReferenceFrame('A')
B = A.orientnew('B', 'body', [q1, q2, q3], 'xyz')
# define points
pP = Point('P')
pP.set_vel(A, 0)
pB_star = pP.locatenew('B*', q4 * A.x)
pB_star.set_vel(B, 0)
pB_star.set_vel(A, pB_star.pos_from(pP).dt(A))
# kinematic differential equations
kde = [x - y for x, y in zip([u1, u2, u3], map(B.ang_vel_in(A).dot, B))]
kde += [u4 - q4d]
kde_map = solve(kde, [q1d, q2d, q3d, q4d])
I = inertia(B, I1, I2, I3) # central inertia dyadic of B
# forces, torques due to set of gravitational forces γ
C11, C12, C13, C21, C22, C23, C31, C32, C33 = [dot(x, y) for x in A for y in B]
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, parameters=None, samples=None):
## coordinates
# theta: pitch
# psi: yaw
# phi: roll
# delta: steer
# these correspond to x_aux[:] + x[0:3] in the auxiliary state and
# state vectors
x, y, theta, psi, phi, delta = symbols('x y θ ψ φ δ')
# rr: rear radius
# rf: front radius
rr, rf = symbols('rr rf')
# d1: orthogonal distance from rear wheel center to steer axis
# d3: orthogonal distance from front wheel center to steer axis
# d2: steer axis separation
d1, d2, d3 = symbols('d1:4')
## reference frames
# N: inertial frame
# D: rear wheel frame
# C: rear assembly frame - yaw, roll, pitch from inertial frame
# E: front assembly frame
# F: front wheel frame
N = ReferenceFrame('N')
C = N.orientnew('C', 'body', [psi, phi, theta], 'zxy')
E = C.orientnew('E', 'axis', [delta, C.z]) # steer
# Since it will not matter if circles (wheels) rotate, we can attach
# them to the respective assembly frames
# However, we need to rotate the rear/front assembly frames for a
# cylinder shape to be attached correctly. We can simply rotate pi/2
# about the z-axis for C and about the x-axis for E.
Cy = C.orientnew('Cy', 'axis', [pi/2, C.z])
Ey = C.orientnew('Ey', 'axis', [pi/2, E.x])
## points
# define unit vectors from rear/front wheel centers to ground in the
# wheel plane which will be used to describe points
Dz = ((C.y ^ N.z) ^ C.y).normalize()
Fz = ((E.y ^ N.z) ^ E.y).normalize()
# origin of inertial frame
O = Point('O')
# rear wheel/ground contact point
dn = O.locatenew('dn', x*N.x + y*N.y)
# rear wheel center point
do = dn.locatenew('do', -rr*Dz)
# orthogonal projection of rear wheel center on steer axis
ce = do.locatenew('ce', d1*C.x)
# front wheel center point
fo = ce.locatenew('fo', d2*C.z + d3*E.x)
# front wheel/ground contact point
fn = fo.locatenew('fn', rf*Fz)
## define additional points for visualization
# rear assembly center
cc = do.locatenew('cc', d1/2*C.x)
# front assembly center
ec = ce.locatenew('ec', d2/2*C.z)
## shapes
Ds = Cylinder(name='rear wheel', radius=rr, length=0.01)
Cs = Cylinder(name='rear assembly', radius=0.02, length=d1,
color='lightseagreen')
Es = Cylinder(name='front assembly', radius=0.02, length=d2,
color='lightseagreen')
Fs = Cylinder(name='front wheel', radius=rf, length=0.01)
## visualization frames
Dv = VisualizationFrame('Rear Wheel', C, do, Ds)
Cv = VisualizationFrame('Rear Assembly', Cy, cc, Cs)
Ev = VisualizationFrame('Front Assembly', Ey, ec, Es)
Fv = VisualizationFrame('Front Wheel', E, fo, Fs)
# V: visualization frame
V = N.orientnew('V', 'axis', [pi, N.x])
self.coordinate = Coordinates(x, y, theta, psi, phi, delta)
self.parameter = Parameters(rr, rf, d1, d2, d3)
self.frame = Frames(N, V, C, E)
self.shape = Visual(Ds, Cs, Es, Fs)
self.point = Visual(do, cc, ec, fo)
self.vframe = Visual(Dv, Cv, Ev, Fv)
self.scene = Scene(V, dn) # scene moves with rear contact point
self.scene.visualization_frames = list(self.vframe.__dict__.values())
self.scene.states_symbols = list(self.coordinate.__dict__.values())
if parameters is not None:
self.set_parameters(parameters)
if samples is not None:
self.set_trajectory(samples)
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)
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_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, manual 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))) # noqa:F841
# 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) # noqa:F841
# 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]) # noqa:F841
# 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 warns_deprecated_sympy():
(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()))
syms_in_forcing = find_dynamicsymbols(kane.forcing)
for qdi in qd:
assert qdi not in syms_in_forcing
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.x))
#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)
#create End Effector
EE = Point('E')
#COM locations
j0_com_length, j1_com_length, j2_com_length = symbols('d_j0, d_j1, d_j2')
j0_mass_center = Point('j0_o')
j0_mass_center.set_pos(joint0, j0_com_length * j0_frame.y)
j1_mass_center = Point('j1_o')
j1_mass_center.set_pos(joint1, j1_com_length * j1_frame.y)
def get_model(model_name, **opts):
"""
model: string defining model
opts: dictionary of options with keys:
see _defaultOpts
"""
for k, v in _defaultOpts.items():
if k not in opts.keys():
opts[k] = v
for k, v in opts.items():
if k not in _defaultOpts.keys():
raise Exception(
'Key {} not supported for model options.'.format(k))
#print(opts)
# Extract info from model name
sFnd = model_name.split('T')[0][1:]
if len(sFnd) == 1:
nDOF_fnd = int(sFnd[0])
bFndDOFs = [False] * 6
bFndDOFs[0] = nDOF_fnd >= 1 # x
bFndDOFs[4] = nDOF_fnd >= 2 # phiy
bFndDOFs[2] = nDOF_fnd == 3 or nDOF_fnd == 6 # z
bFndDOFs[1] = nDOF_fnd >= 5 # y
bFndDOFs[3] = nDOF_fnd >= 5 # phi_x
bFndDOFs[5] = nDOF_fnd >= 5 # phi_z
else:
bFndDOFs = [s == '1' for s in sFnd]
nDOF_fnd = sum(bFndDOFs)
nDOF_twr = int(model_name.split('T')[1][0])
bFullRNA = model_name.find('RNA') == -1
bBlades = False # Rotor
nDOF_nac = 0
nDOF_sft = 0
nDOF_bld = 0
if bFullRNA:
nDOF_nac = int(model_name.split('N')[1][0])
nDOF_sft = int(model_name.split('S')[1][0])
bBlades = model_name.find('B') > 0
if bBlades:
nDOF_bld = model_name.split('B')[1][0]
else:
nDOF_bld = 0
#print('fnd',','.join(['1' if b else '0' for b in bFndDOFs]), 'twr',nDOF_twr, 'nac',nDOF_nac, 'sft',nDOF_sft, 'bld',nDOF_bld)
# --------------------------------------------------------------------------------}
# --- Isolated bodies
# --------------------------------------------------------------------------------{
# Reference frame
ref = YAMSInertialBody('E')
# Foundation, floater, always rigid for now
if (not opts['floating']) or opts['mergeFndTwr']:
fnd = None # the floater is merged with the twr, or we are not floating
else:
fnd = YAMSRigidBody('F', rho_G=[0, 0, z_FG], J_diag=True)
# Tower
if nDOF_twr == 0:
# Ridid tower
twr = YAMSRigidBody('T', rho_G=[0, 0, z_TG], J_diag=True)
twrDOFs = []
twrSpeeds = []
elif nDOF_twr <= 4:
# Flexible tower
twr = YAMSFlexibleBody('T',
nDOF_twr,
directions=opts['twrDOFDir'],
orderMM=opts['orderMM'],
orderH=opts['orderH'],
predefined_kind='twr-z')
twrDOFs = twr.q
twrSpeeds = twr.qd
# Nacelle rotor assembly
if bFullRNA:
# Nacelle
nac = YAMSRigidBody('N', rho_G=[x_NG, 0, z_NG], J_cross=True)
# Shaft
#...
# Individual blades or rotor
if bBlades:
# Individual blades
raise NotImplementedError()
else:
# Rotor
rot = YAMSRigidBody('R', rho_G=[0, 0, 0], J_diag=True)
rot.inertia = (inertia(rot.frame, Jxx_R, JO_R, JO_R), rot.origin
) # defining inertia at orign
else:
# Nacelle
#nac = YAMSRigidBody('RNA', rho_G = [x_RNAG ,0, z_RNAG], J_diag=True)
nac = YAMSRigidBody('RNA', rho_G=[x_RNAG, 0, z_RNAG], J_cross=True)
rot = None
# --------------------------------------------------------------------------------}
# --- Body DOFs
# --------------------------------------------------------------------------------{
# Fnd
if (not opts['floating']):
fndDOFs = []
fndSpeeds = []
else:
fndDOFsAll = [x, y, z, phi_x, phi_y, phi_z]
fndSpeedsAll = [xd, yd, zd, omega_x_T, omega_y_T, omega_z_T]
fndDOFs = [dof for active, dof in zip(bFndDOFs, fndDOFsAll) if active]
fndSpeeds = [
dof for active, dof in zip(bFndDOFs, fndSpeedsAll) if active
]
# Twr
# Nac
if nDOF_nac == 2:
opts['yaw'] = 'dynamic'
opts['tilt'] = 'dynamic'
if opts['tiltShaft'] and opts['tilt'] == 'dynamic':
raise Exception('Cannot do tiltshaft with tilt dynamic')
yawDOF = {'zero': 0, 'fixed': theta_yaw, 'dynamic': q_yaw}[opts['yaw']]
tiltDOF = {'zero': 0, 'fixed': theta_tilt, 'dynamic': q_tilt}[opts['tilt']]
nacDOFs = []
nacSpeeds = []
nacKDEqSubs = []
if opts['yaw'] == 'dynamic':
nacDOFs += [q_yaw]
nacSpeeds += [qd_yaw]
nacKDEqSubs += [(qd_yaw, diff(q_yaw, time))]
if opts['tilt'] == 'dynamic':
nacDOFs += [q_tilt]
nacSpeeds += [qd_tilt]
nacKDEqSubs += [(qd_tilt, diff(q_tilt, time))]
nacDOFsAct = (opts['yaw'] == 'dynamic', opts['tilt'] == 'dynamic')
if nDOF_nac == 0:
if not (nacDOFsAct == (False, False)):
raise Exception(
'If nDOF_nac is 0, yaw and tilt needs to be "fixed" or "zero"')
elif nDOF_nac == 1:
if not (nacDOFsAct == (True, False) or nacDOFsAct == (False, True)):
raise Exception(
'If nDOF_nac is 1, yaw or tilt needs to be "dynamic"')
else:
if not (nacDOFsAct == (True, True)):
raise Exception(
'If nDOF_nac is 2, yaw and tilt needs to be "dynamic"')
# Shaft
sftDOFs = []
sftSpeeds = []
if bFullRNA:
if nDOF_sft == 1:
sftDOFs = [psi]
sftSpeeds = [omega_x_R]
elif nDOF_sft == 0:
pass
else:
raise Exception('nDOF shaft should be 0 or 1')
# Blade Rotor
if bBlades:
pass
else:
pass
coordinates = fndDOFs + twrDOFs + nacDOFs + sftDOFs
speeds = fndSpeeds + twrSpeeds + nacSpeeds + sftSpeeds # Order determine eq order
# --------------------------------------------------------------------------------}
# --- Connections between bodies
# --------------------------------------------------------------------------------{
z_OT = Symbol('z_OT')
if opts['floating']:
rel_pos = [0, 0, 0]
rel_pos[0] = x if bFndDOFs[0] else 0
rel_pos[1] = y if bFndDOFs[1] else 0
rel_pos[2] = z + z_OT if bFndDOFs[2] else z_OT
rots = [0, 0, 0]
rots[0] = phi_x if bFndDOFs[3] else 0
rots[1] = phi_y if bFndDOFs[4] else 0
rots[2] = phi_z if bFndDOFs[5] else 0
if nDOF_fnd == 0:
#print('Rigid connection ref twr', rel_pos)
ref.connectTo(twr, type='Rigid', rel_pos=rel_pos)
elif nDOF_fnd == 1:
#print('Constraint connection ref twr')
ref.connectTo(twr,
type='Free',
rel_pos=(x, 0, z_OT),
rot_amounts=(0, x * symbols('nu'), 0),
rot_order='XYZ')
else:
#print('Free connection ref twr', rel_pos, rots)
ref.connectTo(
twr,
type='Free',
rel_pos=rel_pos,
rot_amounts=rots,
rot_order='XYZ'
) #NOTE: rot order is not "optimal".. phi_x should be last
#ref.connectTo(twr, type='Free' , rel_pos=rel_pos, rot_amounts=(rots[2],rots[1],rots[0]), rot_order='ZYX') #NOTE: rot order is not "optimal".. phi_x should be last
else:
#print('Rigid connection ref twr')
ref.connectTo(twr, type='Rigid', rel_pos=(0, 0, 0))
# Rigid connection between twr and fnd if fnd exists
if fnd is not None:
#print('Rigid connection twr fnd')
if nDOF_twr == 0:
twr.connectTo(fnd, type='Rigid', rel_pos=(0, 0, 0)) # -L_F
else:
twr.connectTo(fnd, type='Rigid', rel_pos=(0, 0, 0)) # -L_F
if nDOF_twr == 0:
# Tower rigid -> Rigid connection to nacelle
# TODO TODO L_T or twr.L
#if nDOF_nac==0:
#print('Rigid connection twr nac')
#else:
#print('Dynamic connection twr nac')
if opts['tiltShaft']:
# Shaft will be tilted, not nacelle
twr.connectTo(nac,
type='Rigid',
rel_pos=(0, 0, L_T),
rot_amounts=(yawDOF, 0, 0),
rot_order='ZYX')
else:
# Nacelle is tilted
twr.connectTo(nac,
type='Rigid',
rel_pos=(0, 0, L_T),
rot_amounts=(yawDOF, tiltDOF, 0),
rot_order='ZYX')
else:
# Flexible tower -> Flexible connection to nacelle
#print('Flexible connection twr nac')
if opts['tiltShaft']:
twr.connectToTip(nac,
type='Joint',
rel_pos=(0, 0, twr.L),
rot_amounts=(yawDOF, 0, 0),
rot_order='ZYX',
rot_type_elastic=opts['rot_elastic_type'],
doSubs=opts['rot_elastic_subs'])
else:
twr.connectToTip(nac,
type='Joint',
rel_pos=(0, 0, twr.L),
rot_amounts=(yawDOF, tiltDOF, 0),
rot_order='ZYX',
rot_type_elastic=opts['rot_elastic_type'],
doSubs=opts['rot_elastic_subs'])
if bFullRNA:
if bBlades:
raise NotImplementedError()
else:
if opts['tiltShaft']:
if nDOF_sft == 0:
nac.connectTo(rot,
type='Joint',
rel_pos=(x_NR, 0, z_NR),
rot_amounts=(0, tiltDOF, 0),
rot_order='ZYX')
else:
nac.connectTo(rot,
type='Joint',
rel_pos=(x_NR, 0, z_NR),
rot_amounts=(0, tiltDOF, psi),
rot_order='ZYX')
else:
if nDOF_sft == 0:
nac.connectTo(rot,
type='Joint',
rel_pos=(x_NR, 0, z_NR),
rot_amounts=(0, 0, 0),
rot_order='ZYX')
else:
nac.connectTo(rot,
type='Joint',
rel_pos=(x_NR, 0, z_NR),
rot_amounts=(0, 0, psi),
rot_order='ZYX')
# --- Defining Body rotational velocities
omega_TE = twr.ang_vel_in(
ref) # Angular velocity of nacelle in inertial frame
omega_NT = nac.ang_vel_in(
twr.frame) # Angular velocity of nacelle in inertial frame
if rot is not None:
omega_RN = rot.ang_vel_in(
nac.frame
) # Angular velocity of rotor wrt Nacelle (omega_R-omega_N)
# --- Kinetics
body_loads = []
bodies = []
g = symbols('g')
g_vect = -g * ref.frame.z
# Foundation/floater loads
if fnd is not None:
bodies += [fnd]
grav_F = (fnd.masscenter, -fnd.mass * g * ref.frame.z)
# Point of application for Buoyancy and mooring
P_B = twr.origin.locatenew('P_B',
z_TB * fnd.frame.z) # <<<< Measured from T
P_M = twr.origin.locatenew('P_M',
z_TM * fnd.frame.z) # <<<< Measured from T
P_B.v2pt_theory(twr.origin, ref.frame, twr.frame)
# PB & T are fixed in e_T
P_M.v2pt_theory(twr.origin, ref.frame, twr.frame)
# PM & T are fixed in e_T
if opts['fnd_loads']:
K_Mx, K_My, K_Mz = symbols(
'K_x_M, K_y_M, K_z_M') # Mooring restoring
K_Mphix, K_Mphiy, K_Mphiz = symbols(
'K_phi_x_M, K_phi_y_M, K_phi_z_M') # Mooring restoring
F_B = dynamicsymbols('F_B') # Buoyancy force
#print('>>> Adding fnd loads. NOTE: reference might need to be adapted')
# Buoyancy
body_loads += [(fnd, (P_B, F_B * ref.frame.z))]
## Restoring mooring and torques
fr = 0
fr += -K_Mx * x * ref.frame.x if bFndDOFs[0] else 0
fr += -K_My * y * ref.frame.y if bFndDOFs[1] else 0
fr += -K_Mz * z * ref.frame.z if bFndDOFs[2] else 0
Mr += -K_MPhix * phi_x * ref.frame.x if bFndDOFs[3] else 0
Mr += -K_MPhiy * phi_y * ref.frame.y if bFndDOFs[4] else 0
Mr += -K_MPhiz * phi_z * ref.frame.z if bFndDOFs[5] else 0
body_loads += [(fnd, (P_M, fr))]
body_loads += [(fnd, (fnd.frame, Mr))]
body_loads += [(fnd, grav_F)]
# Tower loads
grav_T = (twr.masscenter, -twr.mass * g * ref.frame.z)
bodies += [twr]
body_loads += [(twr, grav_T)]
# Nacelle loads
grav_N = (nac.masscenter, -nac.mass * g * ref.frame.z)
bodies += [nac]
body_loads += [(nac, grav_N)]
T_a = dynamicsymbols('T_a') # NOTE NOTE
#T_a = Function('T_a')(dynamicsymbols._t, *coordinates, *speeds) # NOTE: to introduce it in the linearization, add coordinates
M_ax, M_ay, M_az = dynamicsymbols('M_x_a, M_y_a, M_z_a') # Aero torques
if bFullRNA:
if bBlades:
raise NotImplementedError()
else:
# Rotor loads
grav_R = (rot.masscenter, -M_R * g * ref.frame.z)
bodies += [rot]
body_loads += [(rot, grav_R)]
# NOTE: loads on rot, but expressed in N frame
if opts['tiltShaft']:
# TODO actually tilt shaft, introduce non rotating shaft body
if opts['aero_forces']:
thrustR = (rot.origin, T_a * cos(tiltDOF) * nac.frame.x -
T_a * sin(tiltDOF) * nac.frame.z)
if opts['aero_torques']:
M_a_R = (nac.frame, M_ax * nac.frame.x * 0) # TODO TODO
#print('>>> WARNING tilt shaft aero moments not implemented') # TODO
body_loads += [(nac, M_a_R)]
else:
thrustR = (rot.origin, T_a * nac.frame.x)
#thrustR = (rot.origin, T_a * rot.frame.x )
#M_a_R = (rot.frame, M_ax*rot.frame.x + M_ay*rot.frame.y + M_az*rot.frame.z) # TODO TODO TODO introduce a non rotating shaft
if opts['aero_torques']:
M_a_R = (nac.frame, M_ax * nac.frame.x +
M_ay * nac.frame.y + M_az * nac.frame.z)
body_loads += [(nac, M_a_R)]
body_loads += [(rot, thrustR)]
else:
# RNA loads, point load at R
R = Point('R')
R.set_pos(nac.origin, x_NR * nac.frame.x + z_NR * nac.frame.z)
R.set_vel(nac.frame, 0 * nac.frame.x)
R.v2pt_theory(nac.origin, ref.frame, nac.frame)
#thrustN = (nac.masscenter, T * nac.frame.x)
if opts['tiltShaft']:
thrustN = (R, T_a * cos(tiltDOF) * nac.frame.x -
T_a * sin(tiltDOF) * nac.frame.z)
else:
thrustN = (R, T_a * nac.frame.x)
if opts['aero_forces']:
body_loads += [(nac, thrustN)]
if opts['aero_torques']:
#print('>>> Adding aero torques')
if opts['tiltShaft']:
# NOTE: for a rigid RNA we keep only M_y and M_z, no shaft torque
x_tilted = cos(tiltDOF) * nac.frame.x - sin(
tiltDOF) * nac.frame.z
z_tilted = cos(tiltDOF) * nac.frame.y + sin(
tiltDOF) * nac.frame.x
M_a_N = (nac.frame, M_ay * nac.frame.y + M_az * z_tilted)
else:
M_a_N = (nac.frame, M_ax * nac.frame.x + M_ay * nac.frame.y +
M_az * nac.frame.z)
body_loads += [(nac, M_a_N)]
# --------------------------------------------------------------------------------}
# --- Kinematic equations
# --------------------------------------------------------------------------------{
kdeqsSubs = []
# --- Fnd
if not opts['floating']:
pass
else:
# Kdeqs for fnd:
# : (xd, diff(x,time)) and (omega_y_T, diff(phi_y,time))
fndVelAll = [diff(x, time), diff(y, time), diff(z, time)]
if opts['linRot']:
fndVelAll += [
diff(phi_x, time),
diff(phi_y, time),
diff(phi_z, time)
]
else:
#print('>>>>>>>> TODO sort out which frame')
#fndVelAll +=[ omega_TE.dot(ref.frame.x).simplify(), omega_TE.dot(ref.frame.y).simplify(), omega_TE.dot(ref.frame.z).simplify()]
fndVelAll += [
omega_TE.dot(twr.frame.x).simplify(),
omega_TE.dot(twr.frame.y).simplify(),
omega_TE.dot(twr.frame.z).simplify()
]
kdeqsSubs += [(fndSpeedsAll[i], fndVelAll[i])
for i, dof in enumerate(bFndDOFs) if dof]
# --- Twr
if nDOF_twr == 0:
pass
else:
kdeqsSubs += [(twr.qd[i], twr.qdot[i]) for i, _ in enumerate(twr.q)]
# --- Nac
kdeqsSubs += nacKDEqSubs
# --- Shaft
if bFullRNA:
if bBlades:
raise NotImplementedError()
else:
if nDOF_sft == 1:
#print('>>>>>>>> TODO sort out which frame')
# I believe we should use omega_RE
kdeqsSubs += [(omega_x_R, omega_RN.dot(rot.frame.x).simplify())
]
# --- Create a YAMS wrapper model
model = YAMSModel(name=model_name)
model.opts = opts
model.ref = ref
model.bodies = bodies
model.body_loads = body_loads
model.coordinates = coordinates
model.speeds = speeds
model.kdeqsSubs = kdeqsSubs
#print(model)
model.fnd = fnd
model.twr = twr
model.nac = nac
model.rot = rot
#model.sft=sft
#model.bld=bld
model.g_vect = g_vect
# Small angles
model.smallAnglesFnd = [phi_x, phi_y, phi_z]
if nDOF_twr > 0:
if opts['rot_elastic_smallAngle']:
model.smallAnglesTwr = twr.vcList
else:
model.smallAnglesTwr = []
else:
model.smallAnglesTwr = []
model.smallAnglesNac = []
if opts['yaw'] == 'dynamic':
model.smallAnglesNac += [q_yaw]
if opts['tilt'] == 'dynamic':
model.smallAnglesNac += [q_tilt]
model.smallAngles = model.smallAnglesFnd + model.smallAnglesTwr + model.smallAnglesNac
# Shape normalization
if nDOF_twr > 0:
model.shapeNormSubs = [(v, 1) for v in twr.ucList]
else:
model.shapeNormSubs = []
return model
def test_linearize_rolling_disc_kane():
# Symbols for time and constant parameters
t, r, m, g, v = symbols('t r m g v')
# Configuration variables and their time derivatives
q1, q2, q3, q4, q5, q6 = q = dynamicsymbols('q1:7')
q1d, q2d, q3d, q4d, q5d, q6d = qd = [qi.diff(t) for qi in q]
# Generalized speeds and their time derivatives
u = dynamicsymbols('u:6')
u1, u2, u3, u4, u5, u6 = u = dynamicsymbols('u1:7')
u1d, u2d, u3d, u4d, u5d, u6d = [ui.diff(t) for ui in u]
# Reference frames
N = ReferenceFrame('N') # Inertial frame
NO = Point('NO') # Inertial origin
A = N.orientnew('A', 'Axis', [q1, N.z]) # Yaw intermediate frame
B = A.orientnew('B', 'Axis', [q2, A.x]) # Lean intermediate frame
C = B.orientnew('C', 'Axis', [q3, B.y]) # Disc fixed frame
CO = NO.locatenew('CO', q4 * N.x + q5 * N.y + q6 * N.z) # Disc center
# Disc angular velocity in N expressed using time derivatives of coordinates
w_c_n_qd = C.ang_vel_in(N)
w_b_n_qd = B.ang_vel_in(N)
# Inertial angular velocity and angular acceleration of disc fixed frame
C.set_ang_vel(N, u1 * B.x + u2 * B.y + u3 * B.z)
# Disc center velocity in N expressed using time derivatives of coordinates
v_co_n_qd = CO.pos_from(NO).dt(N)
# Disc center velocity in N expressed using generalized speeds
CO.set_vel(N, u4 * C.x + u5 * C.y + u6 * C.z)
# Disc Ground Contact Point
P = CO.locatenew('P', r * B.z)
P.v2pt_theory(CO, N, C)
# Configuration constraint
f_c = Matrix([q6 - dot(CO.pos_from(P), N.z)])
# Velocity level constraints
f_v = Matrix([dot(P.vel(N), uv) for uv in C])
# Kinematic differential equations
kindiffs = Matrix([dot(w_c_n_qd - C.ang_vel_in(N), uv) for uv in B] +
[dot(v_co_n_qd - CO.vel(N), uv) for uv in N])
qdots = solve(kindiffs, qd)
# Set angular velocity of remaining frames
B.set_ang_vel(N, w_b_n_qd.subs(qdots))
C.set_ang_acc(
N,
C.ang_vel_in(N).dt(B) + cross(B.ang_vel_in(N), C.ang_vel_in(N)))
# Active forces
F_CO = m * g * A.z
# Create inertia dyadic of disc C about point CO
I = (m * r**2) / 4
J = (m * r**2) / 2
I_C_CO = inertia(C, I, J, I)
Disc = RigidBody('Disc', CO, C, m, (I_C_CO, CO))
BL = [Disc]
FL = [(CO, F_CO)]
KM = KanesMethod(N, [q1, q2, q3, q4, q5], [u1, u2, u3],
kd_eqs=kindiffs,
q_dependent=[q6],
configuration_constraints=f_c,
u_dependent=[u4, u5, u6],
velocity_constraints=f_v)
with warnings.catch_warnings():
warnings.filterwarnings("ignore", category=SymPyDeprecationWarning)
(fr, fr_star) = KM.kanes_equations(FL, BL)
# Test generalized form equations
linearizer = KM.to_linearizer()
assert linearizer.f_c == f_c
assert linearizer.f_v == f_v
assert linearizer.f_a == f_v.diff(t)
sol = solve(linearizer.f_0 + linearizer.f_1, qd)
for qi in qd:
assert sol[qi] == qdots[qi]
assert simplify(linearizer.f_2 + linearizer.f_3 - fr - fr_star) == Matrix(
[0, 0, 0])
# Perform the linearization
# Precomputed operating point
q_op = {q6: -r * cos(q2)}
u_op = {
u1: 0,
u2: sin(q2) * q1d + q3d,
u3: cos(q2) * q1d,
u4: -r * (sin(q2) * q1d + q3d) * cos(q3),
u5: 0,
u6: -r * (sin(q2) * q1d + q3d) * sin(q3)
}
qd_op = {
q2d: 0,
q4d: -r * (sin(q2) * q1d + q3d) * cos(q1),
q5d: -r * (sin(q2) * q1d + q3d) * sin(q1),
q6d: 0
}
ud_op = {
u1d:
4 * g * sin(q2) / (5 * r) + sin(2 * q2) * q1d**2 / 2 +
6 * cos(q2) * q1d * q3d / 5,
u2d:
0,
u3d:
0,
u4d:
r * (sin(q2) * sin(q3) * q1d * q3d + sin(q3) * q3d**2),
u5d:
r * (4 * g * sin(q2) /
(5 * r) + sin(2 * q2) * q1d**2 / 2 + 6 * cos(q2) * q1d * q3d / 5),
u6d:
-r * (sin(q2) * cos(q3) * q1d * q3d + cos(q3) * q3d**2)
}
A, B = linearizer.linearize(op_point=[q_op, u_op, qd_op, ud_op],
A_and_B=True,
simplify=True)
upright_nominal = {q1d: 0, q2: 0, m: 1, r: 1, g: 1}
# Precomputed solution
A_sol = Matrix([[0, 0, 0, 0, 0, 0, 0, 1], [0, 0, 0, 0, 0, 1, 0, 0],
[0, 0, 0, 0, 0, 0, 1, 0],
[sin(q1) * q3d, 0, 0, 0, 0, -sin(q1), -cos(q1), 0],
[-cos(q1) * q3d, 0, 0, 0, 0,
cos(q1), -sin(q1), 0],
[0, 4 / 5, 0, 0, 0, 0, 0, 6 * q3d / 5],
[0, 0, 0, 0, 0, 0, 0, 0], [0, 0, 0, 0, 0, -2 * q3d, 0, 0]])
B_sol = Matrix([])
# Check that linearization is correct
assert A.subs(upright_nominal) == A_sol
assert B.subs(upright_nominal) == B_sol
# Check eigenvalues at critical speed are all zero:
assert A.subs(upright_nominal).subs(q3d, 1 / sqrt(3)).eigenvals() == {0: 8}
from sympy import sin, cos, symbols, diff, Matrix
from sympy import solve, simplify
from sympy.physics.mechanics import LagrangesMethod, Lagrangian
from sympy.physics.mechanics import ReferenceFrame, Particle, Point
from sympy.physics.mechanics import dynamicsymbols, kinetic_energy
from sympy.physics.mechanics import mprint, mlatex
t = symbols('t')
m1, m2, l, g = symbols('m1 m2 l g') # Time-invariant
p, theta, F = dynamicsymbols('p theta F') # Time-variant
q = Matrix([p, theta])
qd = q.diff(t)
N = ReferenceFrame('N')
P1 = Point('P1')
P2 = Point('P2')
x1 = p
y1 = 0
x2 = p - l * sin(theta)
y2 = l * cos(theta)
# Definition of mass and velocity
Pa1 = Particle('Pa1', P1, m1)
Pa2 = Particle('Pa2', P2, m2)
vx1 = diff(x1, t)
vy1 = diff(y1, t)
vx2 = diff(x2, t)
vy2 = diff(y2, t)
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 == Matrix([[0, 1], [-9.8 / L, 0]])
assert B == Matrix([])
from util import msprint, partial_velocities, generalized_active_forces
## --- Declare symbols ---
u1, u2, u3, u4, u5, u6, u7, u8, u9 = dynamicsymbols('u1:10')
c, R = symbols('c R')
x, y, z, r, phi, theta = symbols('x y z r phi theta')
# --- Reference Frames ---
A = ReferenceFrame('A')
B = ReferenceFrame('B')
C = ReferenceFrame('C')
B.set_ang_vel(A, u1 * B.x + u2 * B.y + u3 * B.z)
C.set_ang_vel(A, u4 * B.x + u5 * B.y + u6 * B.z)
C.set_ang_vel(B, C.ang_vel_in(A) - B.ang_vel_in(A))
pC_star = Point('C*')
pC_star.set_vel(C, 0)
# since radius of cavity is very small, assume C* has zero velocity in B
pC_star.set_vel(B, 0)
pC_star.set_vel(A, u7 * B.x + u8 * B.y + u9 * B.z)
## --- define points P, P' ---
# point on C
pP = pC_star.locatenew('P', x * B.x + y * B.y + z * B.z)
pP.set_vel(C, 0)
pP.v2pt_theory(pC_star, B, C)
pP.v2pt_theory(pC_star, A, C)
# point on B
pP_prime = pP.locatenew("P'", 0)
pP_prime.set_vel(B, 0)
pP_prime.v1pt_theory(pC_star, A, B)
R_Ee: 7.0e6,
R_Mm: 2.0e6,
omega_E: 2e-7,
omega_M: 24e-7,
omega_e: 12e-4,
omega_m: 10e-4
}
# reference frames
S = ReferenceFrame('S')
Q = S.orientnew('Q', 'axis', [0, S.x])
E = Q.orientnew('E', 'axis', [0, S.x])
M = E.orientnew('M', 'axis', [0, S.x])
frames = [S, Q, E, M]
pS = Point('S')
pS.set_acc(S, 0)
pQ = Point('Q')
pQ.set_acc(Q, 0)
pE = Point('E')
pE.set_acc(E, 0)
pM = Point('M')
pM.set_acc(M, 0)
pe = Point('e')
pm = Point('m')
points = [pS, pQ, pE, pM, pe, pm]
# v = ω*R, a = ω**2 * R
pQ.set_acc(S, omega_E**2 * R_SQ * S.x)
pE.set_acc(Q, omega_E**2 * R_QE * S.x)
pM.set_acc(E, omega_M**2 * R_EM * S.x)
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 graivty (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 mass center.
# 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(q3), 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
# mass center 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.mc = Dmc
BodyD.inertia = (I, Dmc)
BodyD.frame = R
BodyD.mass = m
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* fromt the body list, compute the mass matrix and forcing
# terms, then solve for the u dots (time derivatives of the generalized
# speeds).
KM = Kane(N)
KM.coords([q1, q2, q3])
KM.speeds([u1, u2, u3])
KM.kindiffeq(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()
C = symbols('C')
## --- reference frames ---
N = ReferenceFrame('N')
# N.z is points upward (from the immersed end to the surface).
# Let q2 be defined as a rotation about N.x.
R = N.orientnew('R', 'Axis', [q2, N.x])
## --- define buoyancy forces ---
# Assume cross-sectional area A will not significantly affect the
# displaced volume since R is a thin rod.
V = A * q1 / cos(q2)
beta = V * rho * g * N.z
# The buoyancy force acts through the center of buoyancy.
pO = Point('pO') # define point O to be at the surface of the fluid
p1 = pO.locatenew('p1', -q1 * N.z)
p2 = p1.locatenew('p2', q1 / cos(q2) / 2 * R.z)
p1.set_vel(N, p1.pos_from(pO).dt(N))
p2.v2pt_theory(p1, N, R)
forces = [(p2, beta)]
## --- find V ---
# since qid = ui, Fr = -dV/dqr
q = [q1, q2]
partials = partial_velocities([p2], map(diff, q), N)
Fr, _ = generalized_active_forces(partials, forces)
# check if dFr/dqs = dFs/dqr for all r, s = 1, ..., n
for r in range(len(q)):
from sympy.physics.mechanics import dynamicsymbols
from util import msprint, subs, partial_velocities, potential_energy
from util import generalized_active_forces, generalized_active_forces_V
q1, q2 = dynamicsymbols('q1 q2')
q1d, q2d = dynamicsymbols('q1 q2', level=1)
u1, u2 = dynamicsymbols('u1 u2')
b, g, m, L, t = symbols('b g m L t')
E, I = symbols('E I')
# reference frames, points, velocities
N = ReferenceFrame('N')
B = N.orientnew('B', 'Axis',
[-(q2 - q1) / (2 * b), N.y]) # small angle approx.
pO = Point('O') # Point O is where B* would be with zero displacement.
pO.set_vel(N, 0)
# small angle approx.
pB_star = pO.locatenew('B*', -(q1 + q2) / 2 * N.x)
pP1 = pO.locatenew('P1', -q1 * N.x - b * N.z)
pP2 = pO.locatenew('P2', -q2 * N.x + b * N.z)
for p in [pB_star, pP1, pP2]:
p.set_vel(N, p.pos_from(pO).diff(t, N))
# kinematic differential equations
kde = [u1 - q1d, u2 - q2d]
kde_map = solve(kde, [q1d, q2d])
# contact/distance forces
M = lambda qi, qj: 12 * E * I / (L**2) * (L / 3 * (qj - qi) / (2 * b) - qi / 2)
RBC_frame.orient(Hsg_frame,'Axis',(thRBCy,Hsg_frame.y))
RUB_frame.orient(Chassis_frame,'Body',[thRUBx,thRUBy,thRUBz],'XYZ')
RLB_frame.orient(Chassis_frame,'Body',[thRLBx,thRLBy,thRLBz],'XYZ')
Jbar_frame.orient(Chassis_frame,'Body',[thJbarx,thJbary,thJbarz],'XYZ')
LFT_frame.orient(LSpdl_frame,'Axis',(thLFTy,LSpdl_frame.y))
RFT_frame.orient(RSpdl_frame,'Axis',(thRFTy,RSpdl_frame.y))
LRT_frame.orient(Hsg_frame,'Axis',(thLRTy,Hsg_frame.y))
RRT_frame.orient(Hsg_frame,'Axis',(thRRTy,Hsg_frame.y))
LRTeth_frame.orient(Hsg_frame, 'Axis',(thLRTethy, Hsg_frame.y))
RRTeth_frame.orient(Hsg_frame, 'Axis',(thRRTethy, Hsg_frame.y))
# ___________________________________________________________________________________________________________________________
# Point and Location
# Joints
GlobalCoord0 = Point('GlobalCoord0') # Global coordinate system origin location
ChassisC = Point('ChassisC') # Chassis Center, x=0, y=0 on global coordinate system
CGpos = Point('CGpos') # CG position
LUCAMP = Point('LUCAMP') # Control arm midpoints
RUCAMP = Point('RUCAMP')
LLCAMP = Point('LLCAMP')
RLCAMP = Point('RLCAMP')
LUBJ = Point('LUBJ') # Ball joint positions
RUBJ = Point('RUBJ')
LLBJ = Point('LLBJ')
RLBJ = Point('RLBJ')
LFWC = Point('LFWC') # Wheel centers
RFWC = Point('RFWC')
LRWC = Point('LRWC')
RRWC = Point('RRWC')
LFCP = Point('LFCP') # Contact patches
## --- Declare symbols ---
theta = dynamicsymbols('theta')
q1, q2, q3, q4, q5, q6 = dynamicsymbols('q1:7')
q1d, q2d, q3d, q4d, q5d, q6d = dynamicsymbols('q1:7', level=1)
u1, u2, u3 = dynamicsymbols('u1:4')
u_prime, E, R, M, g = symbols('u\' E R M g')
x, y, z, r, theta = symbols('x y z r theta')
alpha, beta = symbols('alpha beta')
# --- Reference Frames ---
C = ReferenceFrame('C')
P = C.orientnew('P', 'axis', [theta, C.x])
P.set_ang_vel(C, u1 * C.x)
## --- define points D, S*, Q on frame A and their velocities ---
pP_star = Point('P*')
pP_star.set_vel(P, 0)
pP_star.set_vel(C, u2 * C.x + u3 * C.y)
pQ = pP_star.locatenew('Q', x * C.x + y * C.y + z * C.z)
pQ.set_vel(P, 0)
pQ.v2pt_theory(pP_star, C, P)
## --- map from cartesian to cylindrical coordinates ---
coord_pairs = [(x, x), (y, r * cos(theta)), (z, r * sin(theta))]
coord_map = dict([(x, x), (y, r * cos(theta)), (z, r * sin(theta))])
J = Matrix([coord_map.values()]).jacobian([x, theta, r])
dJ = trigsimp(J.det())
## --- define contact/distance forces ---
# force for a point on ring R1, R2, R3
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
from sympy.testing.pytest import raises
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)
from sympy.physics.mechanics import dot, dynamicsymbols
from util import partial_velocities, generalized_active_forces
# Define generalized coordinates, speeds, and constants:
q1, q2, q3 = dynamicsymbols('q1:4')
v1, v2, v3 = symbols('v1:4')
m1, m2, C, G = symbols('m1 m2 C G')
zeta = symbols('ζ')
## --- reference frames ---
N = ReferenceFrame('N')
## --- points/particles ---
r_ = q1 * N.x + q2 * N.y + q3 * N.z
v = v1 * N.x + v2 * N.y + v3 * N.z
p1 = Point('p1')
p1.set_vel(N, v)
p2 = p1.locatenew('p2', r_)
p2.set_vel(N, v + r_.dt(N))
## --- define gravitational forces ---
F1 = (G * m1 * m2 / dot(r_, r_)) * (p2.pos_from(p1).normalize())
F2 = -F1
forces = [(p1, F1), (p2, F2)]
## --- find V ---
# since qid = ui, Fr = -dV/dqr
q = [q1, q2, q3]
partials = partial_velocities([p1, p2], map(diff, q), N)
Fr, _ = generalized_active_forces(partials, forces)
def test_bicycle():
if ON_TRAVIS:
skip("Too slow for travis.")
# Code to get equations of motion for a bicycle modeled as in:
# J.P Meijaard, Jim M Papadopoulos, Andy Ruina and A.L Schwab. Linearized
# dynamics equations for the balance and steer of a bicycle: a benchmark
# and review. Proceedings of The Royal Society (2007) 463, 1955-1982
# doi: 10.1098/rspa.2007.1857
# Note that this code has been crudely ported from Autolev, which is the
# reason for some of the unusual naming conventions. It was purposefully as
# similar as possible in order to aide debugging.
# Declare Coordinates & Speeds
# Simple definitions for qdots - qd = u
# Speeds are: yaw frame ang. rate, roll frame ang. rate, rear wheel frame
# ang. rate (spinning motion), frame ang. rate (pitching motion), steering
# frame ang. rate, and front wheel ang. rate (spinning motion).
# Wheel positions are ignorable coordinates, so they are not introduced.
q1, q2, q4, q5 = dynamicsymbols('q1 q2 q4 q5')
q1d, q2d, q4d, q5d = dynamicsymbols('q1 q2 q4 q5', 1)
u1, u2, u3, u4, u5, u6 = dynamicsymbols('u1 u2 u3 u4 u5 u6')
u1d, u2d, u3d, u4d, u5d, u6d = dynamicsymbols('u1 u2 u3 u4 u5 u6', 1)
# Declare System's Parameters
WFrad, WRrad, htangle, forkoffset = symbols('WFrad WRrad htangle forkoffset')
forklength, framelength, forkcg1 = symbols('forklength framelength forkcg1')
forkcg3, framecg1, framecg3, Iwr11 = symbols('forkcg3 framecg1 framecg3 Iwr11')
Iwr22, Iwf11, Iwf22, Iframe11 = symbols('Iwr22 Iwf11 Iwf22 Iframe11')
Iframe22, Iframe33, Iframe31, Ifork11 = symbols('Iframe22 Iframe33 Iframe31 Ifork11')
Ifork22, Ifork33, Ifork31, g = symbols('Ifork22 Ifork33 Ifork31 g')
mframe, mfork, mwf, mwr = symbols('mframe mfork mwf mwr')
# Set up reference frames for the system
# N - inertial
# Y - yaw
# R - roll
# WR - rear wheel, rotation angle is ignorable coordinate so not oriented
# Frame - bicycle frame
# TempFrame - statically rotated frame for easier reference inertia definition
# Fork - bicycle fork
# TempFork - statically rotated frame for easier reference inertia definition
# WF - front wheel, again posses a ignorable coordinate
N = ReferenceFrame('N')
Y = N.orientnew('Y', 'Axis', [q1, N.z])
R = Y.orientnew('R', 'Axis', [q2, Y.x])
Frame = R.orientnew('Frame', 'Axis', [q4 + htangle, R.y])
WR = ReferenceFrame('WR')
TempFrame = Frame.orientnew('TempFrame', 'Axis', [-htangle, Frame.y])
Fork = Frame.orientnew('Fork', 'Axis', [q5, Frame.x])
TempFork = Fork.orientnew('TempFork', 'Axis', [-htangle, Fork.y])
WF = ReferenceFrame('WF')
# Kinematics of the Bicycle First block of code is forming the positions of
# the relevant points
# rear wheel contact -> rear wheel mass center -> frame mass center +
# frame/fork connection -> fork mass center + front wheel mass center ->
# front wheel contact point
WR_cont = Point('WR_cont')
WR_mc = WR_cont.locatenew('WR_mc', WRrad * R.z)
Steer = WR_mc.locatenew('Steer', framelength * Frame.z)
Frame_mc = WR_mc.locatenew('Frame_mc', - framecg1 * Frame.x
+ framecg3 * Frame.z)
Fork_mc = Steer.locatenew('Fork_mc', - forkcg1 * Fork.x
+ forkcg3 * Fork.z)
WF_mc = Steer.locatenew('WF_mc', forklength * Fork.x + forkoffset * Fork.z)
WF_cont = WF_mc.locatenew('WF_cont', WFrad * (dot(Fork.y, Y.z) * Fork.y -
Y.z).normalize())
# Set the angular velocity of each frame.
# Angular accelerations end up being calculated automatically by
# differentiating the angular velocities when first needed.
# u1 is yaw rate
# u2 is roll rate
# u3 is rear wheel rate
# u4 is frame pitch rate
# u5 is fork steer rate
# u6 is front wheel rate
Y.set_ang_vel(N, u1 * Y.z)
R.set_ang_vel(Y, u2 * R.x)
WR.set_ang_vel(Frame, u3 * Frame.y)
Frame.set_ang_vel(R, u4 * Frame.y)
Fork.set_ang_vel(Frame, u5 * Fork.x)
WF.set_ang_vel(Fork, u6 * Fork.y)
# Form the velocities of the previously defined points, using the 2 - point
# theorem (written out by hand here). Accelerations again are calculated
# automatically when first needed.
WR_cont.set_vel(N, 0)
WR_mc.v2pt_theory(WR_cont, N, WR)
Steer.v2pt_theory(WR_mc, N, Frame)
Frame_mc.v2pt_theory(WR_mc, N, Frame)
Fork_mc.v2pt_theory(Steer, N, Fork)
WF_mc.v2pt_theory(Steer, N, Fork)
WF_cont.v2pt_theory(WF_mc, N, WF)
# Sets the inertias of each body. Uses the inertia frame to construct the
# inertia dyadics. Wheel inertias are only defined by principle moments of
# inertia, and are in fact constant in the frame and fork reference frames;
# it is for this reason that the orientations of the wheels does not need
# to be defined. The frame and fork inertias are defined in the 'Temp'
# frames which are fixed to the appropriate body frames; this is to allow
# easier input of the reference values of the benchmark paper. Note that
# due to slightly different orientations, the products of inertia need to
# have their signs flipped; this is done later when entering the numerical
# value.
Frame_I = (inertia(TempFrame, Iframe11, Iframe22, Iframe33, 0, 0, Iframe31), Frame_mc)
Fork_I = (inertia(TempFork, Ifork11, Ifork22, Ifork33, 0, 0, Ifork31), Fork_mc)
WR_I = (inertia(Frame, Iwr11, Iwr22, Iwr11), WR_mc)
WF_I = (inertia(Fork, Iwf11, Iwf22, Iwf11), WF_mc)
# Declaration of the RigidBody containers. ::
BodyFrame = RigidBody('BodyFrame', Frame_mc, Frame, mframe, Frame_I)
BodyFork = RigidBody('BodyFork', Fork_mc, Fork, mfork, Fork_I)
BodyWR = RigidBody('BodyWR', WR_mc, WR, mwr, WR_I)
BodyWF = RigidBody('BodyWF', WF_mc, WF, mwf, WF_I)
# The kinematic differential equations; they are defined quite simply. Each
# entry in this list is equal to zero.
kd = [q1d - u1, q2d - u2, q4d - u4, q5d - u5]
# The nonholonomic constraints are the velocity of the front wheel contact
# point dotted into the X, Y, and Z directions; the yaw frame is used as it
# is "closer" to the front wheel (1 less DCM connecting them). These
# constraints force the velocity of the front wheel contact point to be 0
# in the inertial frame; the X and Y direction constraints enforce a
# "no-slip" condition, and the Z direction constraint forces the front
# wheel contact point to not move away from the ground frame, essentially
# replicating the holonomic constraint which does not allow the frame pitch
# to change in an invalid fashion.
conlist_speed = [WF_cont.vel(N) & Y.x, WF_cont.vel(N) & Y.y, WF_cont.vel(N) & Y.z]
# The holonomic constraint is that the position from the rear wheel contact
# point to the front wheel contact point when dotted into the
# normal-to-ground plane direction must be zero; effectively that the front
# and rear wheel contact points are always touching the ground plane. This
# is actually not part of the dynamic equations, but instead is necessary
# for the lineraization process.
conlist_coord = [WF_cont.pos_from(WR_cont) & Y.z]
# The force list; each body has the appropriate gravitational force applied
# at its mass center.
FL = [(Frame_mc, -mframe * g * Y.z),
(Fork_mc, -mfork * g * Y.z),
(WF_mc, -mwf * g * Y.z),
(WR_mc, -mwr * g * Y.z)]
BL = [BodyFrame, BodyFork, BodyWR, BodyWF]
# The N frame is the inertial frame, coordinates are supplied in the order
# of independent, dependent coordinates, as are the speeds. The kinematic
# differential equation are also entered here. Here the dependent speeds
# are specified, in the same order they were provided in earlier, along
# with the non-holonomic constraints. The dependent coordinate is also
# provided, with the holonomic constraint. Again, this is only provided
# for the linearization process.
KM = KanesMethod(N, q_ind=[q1, q2, q5],
q_dependent=[q4], configuration_constraints=conlist_coord,
u_ind=[u2, u3, u5],
u_dependent=[u1, u4, u6], velocity_constraints=conlist_speed,
kd_eqs=kd)
(fr, frstar) = KM.kanes_equations(FL, BL)
# This is the start of entering in the numerical values from the benchmark
# paper to validate the eigen values of the linearized equations from this
# model to the reference eigen values. Look at the aforementioned paper for
# more information. Some of these are intermediate values, used to
# transform values from the paper into the coordinate systems used in this
# model.
PaperRadRear = 0.3
PaperRadFront = 0.35
HTA = evalf.N(pi / 2 - pi / 10)
TrailPaper = 0.08
rake = evalf.N(-(TrailPaper*sin(HTA)-(PaperRadFront*cos(HTA))))
PaperWb = 1.02
PaperFrameCgX = 0.3
PaperFrameCgZ = 0.9
PaperForkCgX = 0.9
PaperForkCgZ = 0.7
FrameLength = evalf.N(PaperWb*sin(HTA)-(rake-(PaperRadFront-PaperRadRear)*cos(HTA)))
FrameCGNorm = evalf.N((PaperFrameCgZ - PaperRadRear-(PaperFrameCgX/sin(HTA))*cos(HTA))*sin(HTA))
FrameCGPar = evalf.N((PaperFrameCgX / sin(HTA) + (PaperFrameCgZ - PaperRadRear - PaperFrameCgX / sin(HTA) * cos(HTA)) * cos(HTA)))
tempa = evalf.N((PaperForkCgZ - PaperRadFront))
tempb = evalf.N((PaperWb-PaperForkCgX))
tempc = evalf.N(sqrt(tempa**2+tempb**2))
PaperForkL = evalf.N((PaperWb*cos(HTA)-(PaperRadFront-PaperRadRear)*sin(HTA)))
ForkCGNorm = evalf.N(rake+(tempc * sin(pi/2-HTA-acos(tempa/tempc))))
ForkCGPar = evalf.N(tempc * cos((pi/2-HTA)-acos(tempa/tempc))-PaperForkL)
# Here is the final assembly of the numerical values. The symbol 'v' is the
# forward speed of the bicycle (a concept which only makes sense in the
# upright, static equilibrium case?). These are in a dictionary which will
# later be substituted in. Again the sign on the *product* of inertia
# values is flipped here, due to different orientations of coordinate
# systems.
v = symbols('v')
val_dict = {WFrad: PaperRadFront,
WRrad: PaperRadRear,
htangle: HTA,
forkoffset: rake,
forklength: PaperForkL,
framelength: FrameLength,
forkcg1: ForkCGPar,
forkcg3: ForkCGNorm,
framecg1: FrameCGNorm,
framecg3: FrameCGPar,
Iwr11: 0.0603,
Iwr22: 0.12,
Iwf11: 0.1405,
Iwf22: 0.28,
Ifork11: 0.05892,
Ifork22: 0.06,
Ifork33: 0.00708,
Ifork31: 0.00756,
Iframe11: 9.2,
Iframe22: 11,
Iframe33: 2.8,
Iframe31: -2.4,
mfork: 4,
mframe: 85,
mwf: 3,
mwr: 2,
g: 9.81,
q1: 0,
q2: 0,
q4: 0,
q5: 0,
u1: 0,
u2: 0,
u3: v / PaperRadRear,
u4: 0,
u5: 0,
u6: v / PaperRadFront}
# Linearizes the forcing vector; the equations are set up as MM udot =
# forcing, where MM is the mass matrix, udot is the vector representing the
# time derivatives of the generalized speeds, and forcing is a vector which
# contains both external forcing terms and internal forcing terms, such as
# centripital or coriolis forces. This actually returns a matrix with as
# many rows as *total* coordinates and speeds, but only as many columns as
# independent coordinates and speeds.
forcing_lin = KM.linearize()[0]
# As mentioned above, the size of the linearized forcing terms is expanded
# to include both q's and u's, so the mass matrix must have this done as
# well. This will likely be changed to be part of the linearized process,
# for future reference.
MM_full = KM.mass_matrix_full
MM_full_s = MM_full.subs(val_dict)
forcing_lin_s = forcing_lin.subs(KM.kindiffdict()).subs(val_dict)
MM_full_s = MM_full_s.evalf()
forcing_lin_s = forcing_lin_s.evalf()
# Finally, we construct an "A" matrix for the form xdot = A x (x being the
# state vector, although in this case, the sizes are a little off). The
# following line extracts only the minimum entries required for eigenvalue
# analysis, which correspond to rows and columns for lean, steer, lean
# rate, and steer rate.
Amat = MM_full_s.inv() * forcing_lin_s
A = Amat.extract([1, 2, 4, 6], [1, 2, 3, 5])
# Precomputed for comparison
Res = Matrix([[ 0, 0, 1.0, 0],
[ 0, 0, 0, 1.0],
[9.48977444677355, -0.891197738059089*v**2 - 0.571523173729245, -0.105522449805691*v, -0.330515398992311*v],
[11.7194768719633, -1.97171508499972*v**2 + 30.9087533932407, 3.67680523332152*v, -3.08486552743311*v]])
# Actual eigenvalue comparison
eps = 1.e-12
for i in range(6):
error = Res.subs(v, i) - A.subs(v, i)
assert all(abs(x) < eps for x in error)
## --- Declare sy3mbols ---
q1 = dynamicsymbols('q1')
u1, u2, u3 = dynamicsymbols('u1:4')
u_prime, R, M, g, e, f, theta = symbols('u\' R, M, g, e, f, theta')
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 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.set_vel(A, 0)
p.v2pt_theory(pD, F, A)
## --- define partial velocities ---
partials = partial_velocities([pD, pS_star, pQ], [u1, u2, u3],
F, express_frame=A)
forces = [(pS_star, -M*g*F.x), (pQ, Q1*A.x + Q2*A.y + Q3*A.z)]
upper_leg_frame.orient(lower_leg_frame, 'Axis', (theta2, lower_leg_frame.z))
torso_frame = ReferenceFrame('T')
torso_frame.orient(upper_leg_frame, 'Axis', (theta3, upper_leg_frame.z))
# Point Locations
# ===============
# Joints
# ------
lower_leg_length, upper_leg_length = symbols('l_L, l_U')
ankle = Point('A')
knee = Point('K')
knee.set_pos(ankle, lower_leg_length * lower_leg_frame.y)
hip = Point('H')
hip.set_pos(knee, upper_leg_length * upper_leg_frame.y)
# Center of mass locations
# ------------------------
lower_leg_com_length, upper_leg_com_length, torso_com_length = symbols(
'd_L, d_U, d_T')
lower_leg_mass_center = Point('L_o')
lower_leg_mass_center.set_pos(ankle, lower_leg_com_length * lower_leg_frame.y)
from sympy.physics.mechanics import ReferenceFrame, Point
from sympy.physics.mechanics import dynamicsymbols
from util import msprint, subs, partial_velocities
from util import generalized_active_forces, potential_energy
q1, q2, q3, q4, q5, q6 = q = dynamicsymbols('q1:7')
u1, u2, u3, u4, u5, u6 = u = dynamicsymbols('u1:7')
# L' is the natural length of the springs
a, k, L_prime = symbols('a k L\'', real=True, positive=True)
# reference frames
X = ReferenceFrame('X')
C = X.orientnew('C', 'body', [q4, q5, q6], 'xyz')
# define points
pO = Point('O') # point O is fixed in X
pC_star = pO.locatenew('C*', a * (q1 * X.x + q2 * X.y + q3 * X.z))
# define points of the cube connected to springs
pC1 = pC_star.locatenew('C1', a * (C.x + C.y - C.z))
pC2 = pC_star.locatenew('C2', a * (C.y + C.z - C.x))
pC3 = pC_star.locatenew('C3', a * (C.z + C.x - C.y))
# define fixed spring points
pk1 = pO.locatenew('k1', L_prime * X.x + a * (X.x + X.y - X.z))
pk2 = pO.locatenew('k2', L_prime * X.y + a * (X.y + X.z - X.x))
pk3 = pO.locatenew('k3', L_prime * X.z + a * (X.z + X.x - X.y))
pC_star.set_vel(X, pC_star.pos_from(pO).dt(X))
pC1.v2pt_theory(pC_star, X, C)
pC2.v2pt_theory(pC_star, X, C)
pC3.v2pt_theory(pC_star, X, C)
