def test_point_pos():
q = dynamicsymbols('q')
N = ReferenceFrame('N')
B = N.orientnew('B', 'Axis', [q, N.z])
O = Point('O')
P = O.locatenew('P', 10 * N.x + 5 * B.x)
assert P.pos_from(O) == 10 * N.x + 5 * B.x
Q = P.locatenew('Q', 10 * N.y + 5 * B.y)
assert Q.pos_from(P) == 10 * N.y + 5 * B.y
assert Q.pos_from(O) == 10 * N.x + 10 * N.y + 5 * B.x + 5 * B.y
assert O.pos_from(Q) == -10 * N.x - 10 * N.y - 5 * B.x - 5 * B.y
def test_point_pos():
q = dynamicsymbols('q')
N = ReferenceFrame('N')
B = N.orientnew('B', 'Axis', [q, N.z])
O = Point('O')
P = O.locatenew('P', 10 * N.x + 5 * B.x)
assert P.pos_from(O) == 10 * N.x + 5 * B.x
Q = P.locatenew('Q', 10 * N.y + 5 * B.y)
assert Q.pos_from(P) == 10 * N.y + 5 * B.y
assert Q.pos_from(O) == 10 * N.x + 10 * N.y + 5 * B.x + 5 * B.y
assert O.pos_from(Q) == -10 * N.x - 10 * N.y - 5 * B.x - 5 * B.y
def test_center_of_mass():
a = ReferenceFrame('a')
m = symbols('m', real=True)
p1 = Particle('p1', Point('p1_pt'), S.One)
p2 = Particle('p2', Point('p2_pt'), S(2))
p3 = Particle('p3', Point('p3_pt'), S(3))
p4 = Particle('p4', Point('p4_pt'), m)
b_f = ReferenceFrame('b_f')
b_cm = Point('b_cm')
mb = symbols('mb')
b = RigidBody('b', b_cm, b_f, mb, (outer(b_f.x, b_f.x), b_cm))
p2.point.set_pos(p1.point, a.x)
p3.point.set_pos(p1.point, a.x + a.y)
p4.point.set_pos(p1.point, a.y)
b.masscenter.set_pos(p1.point, a.y + a.z)
point_o=Point('o')
point_o.set_pos(p1.point, center_of_mass(p1.point, p1, p2, p3, p4, b))
expr = 5/(m + mb + 6)*a.x + (m + mb + 3)/(m + mb + 6)*a.y + mb/(m + mb + 6)*a.z
assert point_o.pos_from(p1.point)-expr == 0
def test_center_of_mass():
a = ReferenceFrame('a')
m = symbols('m', real=True)
p1 = Particle('p1', Point('p1_pt'), S(1))
p2 = Particle('p2', Point('p2_pt'), S(2))
p3 = Particle('p3', Point('p3_pt'), S(3))
p4 = Particle('p4', Point('p4_pt'), m)
b_f = ReferenceFrame('b_f')
b_cm = Point('b_cm')
mb = symbols('mb')
b = RigidBody('b', b_cm, b_f, mb, (outer(b_f.x, b_f.x), b_cm))
p2.point.set_pos(p1.point, a.x)
p3.point.set_pos(p1.point, a.x + a.y)
p4.point.set_pos(p1.point, a.y)
b.masscenter.set_pos(p1.point, a.y + a.z)
point_o=Point('o')
point_o.set_pos(p1.point, center_of_mass(p1.point, p1, p2, p3, p4, b))
expr = 5/(m + mb + 6)*a.x + (m + mb + 3)/(m + mb + 6)*a.y + mb/(m + mb + 6)*a.z
assert point_o.pos_from(p1.point)-expr == 0
def test_center_of_mass():
a = ReferenceFrame("a")
m = symbols("m", real=True)
p1 = Particle("p1", Point("p1_pt"), S.One)
p2 = Particle("p2", Point("p2_pt"), S(2))
p3 = Particle("p3", Point("p3_pt"), S(3))
p4 = Particle("p4", Point("p4_pt"), m)
b_f = ReferenceFrame("b_f")
b_cm = Point("b_cm")
mb = symbols("mb")
b = RigidBody("b", b_cm, b_f, mb, (outer(b_f.x, b_f.x), b_cm))
p2.point.set_pos(p1.point, a.x)
p3.point.set_pos(p1.point, a.x + a.y)
p4.point.set_pos(p1.point, a.y)
b.masscenter.set_pos(p1.point, a.y + a.z)
point_o = Point("o")
point_o.set_pos(p1.point, center_of_mass(p1.point, p1, p2, p3, p4, b))
expr = (5 / (m + mb + 6) * a.x + (m + mb + 3) / (m + mb + 6) * a.y + mb /
(m + mb + 6) * a.z)
assert point_o.pos_from(p1.point) - expr == 0
# ------
a_length, b_length, c_length = symbols('l_a, l_b, l_c')
a_mass, b_mass, c_mass = symbols('m_a, m_b, m_c')
A = Point('A')
B = Point('B')
C = Point('C')
D = Point('D')
com_global = Point('G_o')
com_bc = Point('BC_o')
B.set_pos(A, a_length * a_frame.y)
C.set_pos(B, b_length * b_frame.y)
D.set_pos(C, c_length * c_frame.y)
t_ai = simplify(B.pos_from(A).express(inertial_frame).to_matrix(inertial_frame))
t_bi = simplify(C.pos_from(A).express(inertial_frame).to_matrix(inertial_frame))
t_ci = simplify(D.pos_from(A).express(inertial_frame).to_matrix(inertial_frame))
t_ba = simplify(C.pos_from(B).express(inertial_frame).to_matrix(inertial_frame))
t_ca = simplify(D.pos_from(B).express(inertial_frame).to_matrix(inertial_frame))
com_global_coords = (t_ai*a_mass + t_bi*b_mass + t_ci*c_mass)/(a_mass+b_mass+c_mass)
com_bc_coords = (t_ba*b_mass + t_ba*c_mass)/(b_mass+c_mass)
theta_go = atan(com_global_coords[0]/com_global_coords[1])
theta_bco = atan(com_bc_coords[0]/com_bc_coords[1])
go_frame.orient(inertial_frame, 'Axis', (theta_go, inertial_frame.z))
bco_frame.orient(b_frame, 'Axis', (theta_bco, inertial_frame.z))
wy: dot(qd[0]*Y.z, R.y),
wz: dot(qd[0]*Y.z, R.z)}
v_ro_n = cross(w_r_n, RO.pos_from(P))
a_ro_n = cross(w_r_n, v_ro_n)
mu_dict = {mu_x: dot(R.x, Y.z), mu_y: dot(R.y, Y.z), mu_z: dot(R.z, Y.z)}
#F_RO = m*g*Y.z + Fx*Y.x + Fy*Y.y + Fz*Y.z
#F_RO = Fx*R.x + Fy*R.y + Fz*R.z + m*g*Y.z
F_RO = Fx*R.x + Fy*R.y + Fz*R.z + m*g*(mu_x*R.x + mu_y*R.y + mu_z*R.z)
newton_eqn = F_RO - m*a_ro_n
force_scalars = solve([dot(newton_eqn, uv).expand() for uv in R], [Fx, Fy, Fz])
#print("v_ro_n =", v_ro_n)
#print("a_ro_n =", a_ro_n)
#print("Force scalars =", force_scalars)
euler_eqn = cross(P.pos_from(RO), F_RO) - cross(w_r_n, dot(I, w_r_n))
#print(euler_eqn)
print(dot(euler_eqn, R.x).subs(omega_dict).expand())
print(dot(euler_eqn, R.y).subs(omega_dict).expand())
print(dot(euler_eqn, R.z).subs(omega_dict).expand().subs(force_scalars).expand().subs(mu_dict).expand())
# Mass center position and velocity
RO = O.locatenew('RO', d*R.x + e*R.y + f*R.z)
RO.set_vel(N, v_ro_n)
O.v2pt_theory(RO, N, R)
# Partial angular velocities and partial velocities
partial_w = [R.ang_vel_in(N).diff(qdi, N) for qdi in qd]
partial_v_RO = [RO.vel(N).diff(qdi, N) for qdi in qd]
steady_dict = {qd[1]: 0, qd[2]: 0,
}
v_ro_n = cross(w_r_n, RO.pos_from(P))
a_ro_n = cross(w_r_n, v_ro_n)
mu_dict = {mu_x: dot(R.x, Y.z), mu_y: dot(R.y, Y.z), mu_z: dot(R.z, Y.z)}
#F_RO = m*g*Y.z + Fx*Y.x + Fy*Y.y + Fz*Y.z
#F_RO = Fx*R.x + Fy*R.y + Fz*R.z + m*g*Y.z
F_RO = Fx * R.x + Fy * R.y + Fz * R.z + m * g * (mu_x * R.x + mu_y * R.y +
mu_z * R.z)
newton_eqn = F_RO - m * a_ro_n
force_scalars = solve([dot(newton_eqn, uv).expand() for uv in R], [Fx, Fy, Fz])
#print("v_ro_n =", v_ro_n)
#print("a_ro_n =", a_ro_n)
#print("Force scalars =", force_scalars)
euler_eqn = cross(P.pos_from(RO), F_RO) - cross(w_r_n, dot(I, w_r_n))
#print(euler_eqn)
print(dot(euler_eqn, R.x).subs(omega_dict).expand())
print(dot(euler_eqn, R.y).subs(omega_dict).expand())
print(
dot(euler_eqn, R.z).subs(omega_dict).expand().subs(
force_scalars).expand().subs(mu_dict).expand())
# Mass center position and velocity
RO = O.locatenew('RO', d * R.x + e * R.y + f * R.z)
RO.set_vel(N, v_ro_n)
O.v2pt_theory(RO, N, R)
# Partial angular velocities and partial velocities
partial_w = [R.ang_vel_in(N).diff(qdi, N) for qdi in qd]
partial_v_RO = [RO.vel(N).diff(qdi, N) for qdi in qd]
# David A. Levinson, 1982, International Journal of Non-Linear Mechanics
mu = [dot(rk, Y.z) for rk in R]
eps = sqrt((a*mu[0])**2 + (b*mu[1])**2 + (c*mu[2])**2)
O = P.locatenew('O', -a*a*mu[0]/eps*R.x
-b*b*mu[1]/eps*R.y
-c*c*mu[2]/eps*R.z)
O.set_vel(N, P.vel(N) + cross(R.ang_vel_in(N), O.pos_from(P)))
# Mass center position and velocity
RO = O.locatenew('RO', d*R.x + e*R.y + f*R.z)
RO.set_vel(N, P.vel(N) + cross(R.ang_vel_in(N), RO.pos_from(P)))
qd_rhs = [(u[2]*cos(q[2]) - u[0]*sin(q[2]))/cos(q[1]),
u[0]*cos(q[2]) + u[2]*sin(q[2]),
u[1] + tan(q[1])*(u[0]*sin(q[2]) - u[2]*cos(q[2])),
dot(P.pos_from(O).diff(t, R), N.x),
dot(P.pos_from(O).diff(t, R), N.y)]
# Partial angular velocities and partial velocities
partial_w = [R.ang_vel_in(N).diff(ui, N) for ui in u + ua]
partial_v_P = [P.vel(N).diff(ui, N) for ui in u + ua]
partial_v_RO = [RO.vel(N).diff(ui, N) for ui in u + ua]
# Set auxiliary generalized speeds to zero in velocity vectors
P.set_vel(N, P.vel(N).subs({ua[0]:0, ua[1]:0, ua[2]:0}))
O.set_vel(N, O.vel(N).subs({ua[0]:0, ua[1]:0, ua[2]:0}))
RO.set_vel(N, RO.vel(N).subs({ua[0]:0, ua[1]:0, ua[2]:0}))
# Angular acceleration
R.set_ang_acc(N, ud[0]*R.x + ud[1]*R.y + ud[2]*R.z)
kane = KanesMethod(inertial_frame, coordinates, speeds, kinematic_differential_equations)
loads = [t_grav_force,
s_link_torque]
bodies = [tP]
fr, frstar = kane.kanes_equations(loads, bodies)
mass_matrix = kane.mass_matrix
forcing = kane.forcing[0]
# Constants
# ---------
constants = [s_length,
s_mass,
g]
time = symbols('t')
omega_s = dynamicsymbols('omega_s')
thetadot_omega_dict = dict(zip([theta_s.diff(time)], [omega_s]))
alpha_s = dynamicsymbols('alpha_s')
omegadot_alpha_dict = dict(zip([omega_s.diff(time)], [alpha_s]))
forces = forcing - s_torque
ri = s_frame.dcm(inertial_frame)
com = T.pos_from(S).express(inertial_frame).to_matrix(inertial_frame)
com_dot = com.diff(time).subs(thetadot_omega_dict)
com_ddot = com_dot.diff(time).subs(thetadot_omega_dict).subs(omegadot_alpha_dict)
from sympy import symbols, simplify, latex
from sympy.physics.mechanics import (inertia, inertia_of_point_mass, mprint,
mlatex, Point, ReferenceFrame)
Ixx, Iyy, Izz, Ixz, I, J = symbols("I_xx I_yy I_zz I_xz I J")
mA, mB, lx, lz = symbols("m_A m_B l_x l_z")
A = ReferenceFrame("A")
IA_AO = inertia(A, Ixx, Iyy, Izz, 0, 0, Ixz)
IB_BO = inertia(A, I, J, I)
BO = Point('BO')
AO = BO.locatenew('AO', lx*A.x + lz*A.z)
CO = BO.locatenew('CO', mA / (mA + mB) * AO.pos_from(BO))
print(mlatex(CO.pos_from(BO)))
IC_CO = IA_AO + IB_BO +\
inertia_of_point_mass(mA, AO.pos_from(CO), A) +\
inertia_of_point_mass(mB, BO.pos_from(CO), A)
mprint((A.x & IC_CO & A.x).expand())
mprint((A.y & IC_CO & A.y).expand())
mprint((A.z & IC_CO & A.z).expand())
mprint((A.x & IC_CO & A.z).expand())
#Init joints- points = translation from existing points
ankle = Point('A')
lower_leg_length = symbols('l_L')
knee = Point('K')
knee.set_pos(ankle, lower_leg_length * lower_leg_frame.y)
#pretty_print(knee.pos_from(ankle))
#pretty_print(knee.pos_from(ankle).express(inertial_frame).simplify())
upper_leg_length = symbols('l_U')
hip = Point('H')
hip.set_pos(knee, upper_leg_length * upper_leg_frame.y)
#COM 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)
lower_leg_mass_center.pos_from(ankle)
upper_leg_mass_center = Point('U_o')
upper_leg_mass_center.set_pos(knee, upper_leg_com_length * upper_leg_frame.y)
upper_leg_mass_center.pos_from(ankle)
torso_mass_center = Point('T_o')
torso_mass_center.set_pos(hip, torso_com_length * torso_frame.y)
torso_mass_center.pos_from(ankle)
#kinematic differential equations
#init var for GENERALIZED speeds (aka angular velocities)
omega1, omega2, omega3 = dynamicsymbols('omega1, omega2, omega3')
kinematical_differential_equations = [omega1 - theta1.diff(),
omega2 - theta2.diff(),
omega3 - theta3.diff()]
#pretty_print(kinematical_differential_equations)
# Mass
# ====
a_mass, b_mass, c_mass = symbols("m_a, m_b, m_c")
# Inertia
# =======
rotI = lambda I, f: Matrix([j & I & i for i in f for j in f]).reshape(3, 3)
one_inertia_dyadic_dp1 = inertia(
one_frame_dp1,
0,
0,
rotI(inertia_of_point_mass(a_mass, one_mass_center_dp1.pos_from(one_dp1), one_frame_dp1), one_frame_dp1)[8],
)
one_central_inertia_dp1 = (one_inertia_dyadic_dp1, one_mass_center_dp1)
two_inertia_dyadic_dp1 = inertia(
two_frame_dp1,
0,
0,
rotI(inertia_of_point_mass(b_mass + c_mass, two_mass_center_dp1.pos_from(one_dp1), one_frame_dp1), one_frame_dp1)[
8
],
)
two_central_inertia_dp1 = (two_inertia_dyadic_dp1, two_mass_center_dp1)
CM_1.v2pt_theory(O, B0, B1)
CM_2 = Point('CM_2')
CM_2.set_pos(A, R_2 * B2.x)
CM_2.v2pt_theory(O, B0, B2)
# Corpos Rígidos
I_1 = inertia(B1, 0, 0, I_1_ZZ)
# Elo 1
E_1 = RigidBody('Elo_1', CM_1, B1, M_1, (I_1, CM_1))
I_2 = inertia(B1, 0, 0, I_1_ZZ)
# Elo 2
E_2 = RigidBody('Elo_2', CM_2, B2, M_2, (I_2, CM_2))
# Energia Potencial
P_1 = -M_1 * G * B0.z
R_1_CM = CM_1.pos_from(O).express(B0)
E_1.potential_energy = R_1_CM.dot(P_1)
P_2 = -M_2 * G * B0.z
R_2_CM = CM_2.pos_from(O).express(B0).simplify()
E_2.potential_energy = R_2_CM.dot(P_2)
# Forças/Momentos Generalizados
FL = [(B1, TAU_1 * B0.z), (B2, TAU_2 * B0.z)]
# Método de Lagrange
L = Lagrangian(B0, E_1, E_2)
L = L.simplify()
pprint(L)
LM = LagrangesMethod(L, [THETA_1, THETA_2], frame=B0, forcelist=FL)
# Equações do Movimento
L_EQ = LM.form_lagranges_equations()
from sympy import symbols, simplify, latex
from sympy.physics.mechanics import (inertia, inertia_of_point_mass, mprint,
mlatex, Point, ReferenceFrame)
Ixx, Iyy, Izz, Ixz, I, J = symbols("I_xx I_yy I_zz I_xz I J")
mA, mB, lx, lz = symbols("m_A m_B l_x l_z")
A = ReferenceFrame("A")
IA_AO = inertia(A, Ixx, Iyy, Izz, 0, 0, Ixz)
IB_BO = inertia(A, I, J, I)
BO = Point('BO')
AO = BO.locatenew('AO', lx * A.x + lz * A.z)
CO = BO.locatenew('CO', mA / (mA + mB) * AO.pos_from(BO))
print(mlatex(CO.pos_from(BO)))
IC_CO = IA_AO + IB_BO +\
inertia_of_point_mass(mA, AO.pos_from(CO), A) +\
inertia_of_point_mass(mB, BO.pos_from(CO), A)
mprint((A.x & IC_CO & A.x).expand())
mprint((A.y & IC_CO & A.y).expand())
mprint((A.z & IC_CO & A.z).expand())
mprint((A.x & IC_CO & A.z).expand())
mass_matrix = simplify(kane.mass_matrix)
mass_matrix_full = kane.mass_matrix_full
torques = Matrix(specified)
zero_omega = dict(zip(speeds, zeros(2)))
g_terms = simplify(forcing.subs(zero_omega) - torques)
g_terms_1 = g_terms[0]
g_terms_2 = g_terms[1]
coriolis = simplify(forcing - g_terms - torques)
r_1i = simplify(one_frame.dcm(inertial_frame))
t_1i = simplify(two.pos_from(one).express(inertial_frame).to_matrix(inertial_frame))
t_2i = simplify(three.pos_from(one).express(inertial_frame).to_matrix(inertial_frame))
time = symbols('t')
alpha1, alpha2 = dynamicsymbols('a_1, a_2')
accelerations = [alpha1, alpha2]
thetadot_omega_dict = dict(zip([theta1.diff(time), theta2.diff(time)], [omega1, omega2]))
omegadot_alpha_dict = dict(zip([omega1.diff(time), omega2.diff(time)], [alpha1, alpha2]))
com = (t_1i*one_mass + t_2i*two_mass)/(one_mass+two_mass)
com_dot = com.diff(time).subs(thetadot_omega_dict)
com_ddot = com_dot.diff(time).subs(thetadot_omega_dict).subs(omegadot_alpha_dict).as_mutable()
rx = mu[0]/mu[2]/2/a
ry = mu[1]/mu[2]/2/b
rz = 1 - (mu[0]**2/a + mu[1]**2/b)*(1/2/mu[2])**2
# Locate origin of parabaloid coordinate system relative to contact point
O = P.locatenew('O', -rx*R.x - ry*R.y - rz*R.z)
O.set_vel(N, P.vel(N) + cross(R.ang_vel_in(N), O.pos_from(P)))
# Mass center position and velocity
RO = O.locatenew('RO', d*R.x + e*R.y)
RO.set_vel(N, P.vel(N) + cross(R.ang_vel_in(N), RO.pos_from(P)))
qd_rhs = [(u[2]*cos(q[2]) - u[0]*sin(q[2]))/cos(q[1]),
u[0]*cos(q[2]) + u[2]*sin(q[2]),
u[1] + tan(q[1])*(u[0]*sin(q[2]) - u[2]*cos(q[2])),
dot(P.pos_from(O).diff(t, R), N.x),
dot(P.pos_from(O).diff(t, R), N.y)]
# Partial angular velocities and partial velocities
partial_w = [R.ang_vel_in(N).diff(ui, N) for ui in u + ua]
partial_v_P = [P.vel(N).diff(ui, N) for ui in u + ua]
partial_v_RO = [RO.vel(N).diff(ui, N) for ui in u + ua]
# Set auxiliary generalized speeds to zero in velocity vectors
P.set_vel(N, P.vel(N).subs({ua[0]:0, ua[1]:0, ua[2]:0}))
O.set_vel(N, O.vel(N).subs({ua[0]:0, ua[1]:0, ua[2]:0}))
RO.set_vel(N, RO.vel(N).subs({ua[0]:0, ua[1]:0, ua[2]:0}))
# Angular acceleration
R.set_ang_acc(N, ud[0]*R.x + ud[1]*R.y + ud[2]*R.z)
two.v2pt_theory(one, inertial_frame, one_frame)
two.vel(inertial_frame)
two_mass_center.v2pt_theory(two, inertial_frame, two_frame)
# Mass
# ====
one_mass, two_mass = symbols('m_1, m_2')
# Inertia
# =======
rotI = lambda I, f: Matrix([j & I & i for i in f for j in
f]).reshape(3,3)
one_inertia_dyadic = inertia(one_frame, 0, 0, rotI(inertia_of_point_mass(one_mass, one_mass_center.pos_from(one), one_frame), one_frame)[8])
one_central_inertia = (one_inertia_dyadic, one_mass_center)
two_inertia_dyadic = inertia(two_frame, 0, 0, rotI(inertia_of_point_mass(two_mass, two_mass_center.pos_from(one), one_frame), one_frame)[8])
two_central_inertia = (two_inertia_dyadic, two_mass_center)
# Rigid Bodies
# ============
oneB = RigidBody('One', one_mass_center, one_frame,
one_mass, one_central_inertia)
twoB = RigidBody('Upper Leg', two_mass_center, two_frame,
two_mass, two_central_inertia)
