Python ReferenceFrame.ang_vel_in方法代码示例

# 需要导入模块: from sympy.physics.mechanics import ReferenceFrame [as 别名]
# 或者: from sympy.physics.mechanics.ReferenceFrame import ang_vel_in [as 别名]
l_leg_mass_center.set_pos(l_ankle, l_leg_com_length*l_leg_frame.y)
body_mass_center = Point('B_o')
body_mass_center.set_pos(l_hip, body_com_length*body_frame.y)
r_leg_mass_center = Point('RL_o')
r_leg_mass_center.set_pos(r_hip, l_leg_com_length*r_leg_frame.y)

#Sets up the angular velocities
omega1, omega2, omega3 = dynamicsymbols('omega1, omega2, omega3')
#Relates angular velocity values to the angular positions theta1 and theta2
kinematic_differential_equations = [omega1 - theta1.diff(),
                                    omega2 - theta2.diff(),
                                    omega3 - theta3.diff()]

#Sets up the rotational axes of the angular velocities
l_leg_frame.set_ang_vel(inertial_frame, omega1*inertial_frame.z)
l_leg_frame.ang_vel_in(inertial_frame)
body_frame.set_ang_vel(l_leg_frame, omega2*inertial_frame.z)
body_frame.ang_vel_in(inertial_frame)
r_leg_frame.set_ang_vel(body_frame, omega3*inertial_frame.z)
r_leg_frame.ang_vel_in(inertial_frame)

#Sets up the linear velocities of the points on the linkages
l_ankle.set_vel(inertial_frame, 0)
l_leg_mass_center.v2pt_theory(l_ankle, inertial_frame, l_leg_frame)
l_leg_mass_center.vel(inertial_frame)
l_hip.v2pt_theory(l_ankle, inertial_frame, l_leg_frame)
l_hip.vel(inertial_frame)
body_mass_center.v2pt_theory(l_hip, inertial_frame, body_frame)
body_mass_center.vel(inertial_frame)
r_hip.v2pt_theory(l_hip, inertial_frame, body_frame)
r_hip.vel(inertial_frame)

# 需要导入模块: from sympy.physics.mechanics import ReferenceFrame [as 别名]
# 或者: from sympy.physics.mechanics.ReferenceFrame import ang_vel_in [as 别名]
def test_ang_vel():
    q1, q2, q3, q4 = dynamicsymbols('q1 q2 q3 q4')
    q1d, q2d, q3d, q4d = dynamicsymbols('q1 q2 q3 q4', 1)
    N = ReferenceFrame('N')
    A = N.orientnew('A', 'Axis', [q1, N.z])
    B = A.orientnew('B', 'Axis', [q2, A.x])
    C = B.orientnew('C', 'Axis', [q3, B.y])
    D = N.orientnew('D', 'Axis', [q4, N.y])
    u1, u2, u3 = dynamicsymbols('u1 u2 u3')
    assert A.ang_vel_in(N) == (q1d)*A.z
    assert B.ang_vel_in(N) == (q2d)*B.x + (q1d)*A.z
    assert C.ang_vel_in(N) == (q3d)*C.y + (q2d)*B.x + (q1d)*A.z

    A2 = N.orientnew('A2', 'Axis', [q4, N.y])
    assert N.ang_vel_in(N) == 0
    assert N.ang_vel_in(A) == -q1d*N.z
    assert N.ang_vel_in(B) == -q1d*A.z - q2d*B.x
    assert N.ang_vel_in(C) == -q1d*A.z - q2d*B.x - q3d*B.y
    assert N.ang_vel_in(A2) == -q4d*N.y

    assert A.ang_vel_in(N) == q1d*N.z
    assert A.ang_vel_in(A) == 0
    assert A.ang_vel_in(B) == - q2d*B.x
    assert A.ang_vel_in(C) == - q2d*B.x - q3d*B.y
    assert A.ang_vel_in(A2) == q1d*N.z - q4d*N.y

    assert B.ang_vel_in(N) == q1d*A.z + q2d*A.x
    assert B.ang_vel_in(A) == q2d*A.x
    assert B.ang_vel_in(B) == 0
    assert B.ang_vel_in(C) == -q3d*B.y
    assert B.ang_vel_in(A2) == q1d*A.z + q2d*A.x - q4d*N.y

    assert C.ang_vel_in(N) == q1d*A.z + q2d*A.x + q3d*B.y
    assert C.ang_vel_in(A) == q2d*A.x + q3d*C.y
    assert C.ang_vel_in(B) == q3d*B.y
    assert C.ang_vel_in(C) == 0
    assert C.ang_vel_in(A2) == q1d*A.z + q2d*A.x + q3d*B.y - q4d*N.y

    assert A2.ang_vel_in(N) == q4d*A2.y
    assert A2.ang_vel_in(A) == q4d*A2.y - q1d*N.z
    assert A2.ang_vel_in(B) == q4d*N.y - q1d*A.z - q2d*A.x
    assert A2.ang_vel_in(C) == q4d*N.y - q1d*A.z - q2d*A.x - q3d*B.y
    assert A2.ang_vel_in(A2) == 0

    C.set_ang_vel(N, u1*C.x + u2*C.y + u3*C.z)
    assert C.ang_vel_in(N) == (u1)*C.x + (u2)*C.y + (u3)*C.z
    assert N.ang_vel_in(C) == (-u1)*C.x + (-u2)*C.y + (-u3)*C.z
    assert C.ang_vel_in(D) == (u1)*C.x + (u2)*C.y + (u3)*C.z + (-q4d)*D.y
    assert D.ang_vel_in(C) == (-u1)*C.x + (-u2)*C.y + (-u3)*C.z + (q4d)*D.y

    q0 = dynamicsymbols('q0')
    q0d = dynamicsymbols('q0', 1)
    E = N.orientnew('E', 'Quaternion', (q0, q1, q2, q3))
    assert E.ang_vel_in(N) == (
        2 * (q1d * q0 + q2d * q3 - q3d * q2 - q0d * q1) * E.x +
        2 * (q2d * q0 + q3d * q1 - q1d * q3 - q0d * q2) * E.y +
        2 * (q3d * q0 + q1d * q2 - q2d * q1 - q0d * q3) * E.z)

    F = N.orientnew('F', 'Body', (q1, q2, q3), '313')
    assert F.ang_vel_in(N) == ((sin(q2)*sin(q3)*q1d + cos(q3)*q2d)*F.x +
        (sin(q2)*cos(q3)*q1d - sin(q3)*q2d)*F.y + (cos(q2)*q1d + q3d)*F.z)
    G = N.orientnew('G', 'Axis', (q1, N.x + N.y))
    assert G.ang_vel_in(N) == q1d * (N.x + N.y).normalize()
    assert N.ang_vel_in(G) == -q1d * (N.x + N.y).normalize()

# 需要导入模块: from sympy.physics.mechanics import ReferenceFrame [as 别名]
# 或者: from sympy.physics.mechanics.ReferenceFrame import ang_vel_in [as 别名]
from sympy.physics.mechanics import ReferenceFrame, Point, dynamicsymbols
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)

# 需要导入模块: from sympy.physics.mechanics import ReferenceFrame [as 别名]
# 或者: from sympy.physics.mechanics.ReferenceFrame import ang_vel_in [as 别名]
def test_ang_vel():
    q1, q2, q3, q4 = dynamicsymbols("q1 q2 q3 q4")
    q1d, q2d, q3d, q4d = dynamicsymbols("q1 q2 q3 q4", 1)
    N = ReferenceFrame("N")
    A = N.orientnew("A", "Axis", [q1, N.z])
    B = A.orientnew("B", "Axis", [q2, A.x])
    C = B.orientnew("C", "Axis", [q3, B.y])
    D = N.orientnew("D", "Axis", [q4, N.y])
    u1, u2, u3 = dynamicsymbols("u1 u2 u3")
    assert A.ang_vel_in(N) == (q1d) * A.z
    assert B.ang_vel_in(N) == (q2d) * B.x + (q1d) * A.z
    assert C.ang_vel_in(N) == (q3d) * C.y + (q2d) * B.x + (q1d) * A.z

    A2 = N.orientnew("A2", "Axis", [q4, N.y])
    assert N.ang_vel_in(N) == 0
    assert N.ang_vel_in(A) == -q1d * N.z
    assert N.ang_vel_in(B) == -q1d * A.z - q2d * B.x
    assert N.ang_vel_in(C) == -q1d * A.z - q2d * B.x - q3d * B.y
    assert N.ang_vel_in(A2) == -q4d * N.y

    assert A.ang_vel_in(N) == q1d * N.z
    assert A.ang_vel_in(A) == 0
    assert A.ang_vel_in(B) == -q2d * B.x
    assert A.ang_vel_in(C) == -q2d * B.x - q3d * B.y
    assert A.ang_vel_in(A2) == q1d * N.z - q4d * N.y

    assert B.ang_vel_in(N) == q1d * A.z + q2d * A.x
    assert B.ang_vel_in(A) == q2d * A.x
    assert B.ang_vel_in(B) == 0
    assert B.ang_vel_in(C) == -q3d * B.y
    assert B.ang_vel_in(A2) == q1d * A.z + q2d * A.x - q4d * N.y

    assert C.ang_vel_in(N) == q1d * A.z + q2d * A.x + q3d * B.y
    assert C.ang_vel_in(A) == q2d * A.x + q3d * C.y
    assert C.ang_vel_in(B) == q3d * B.y
    assert C.ang_vel_in(C) == 0
    assert C.ang_vel_in(A2) == q1d * A.z + q2d * A.x + q3d * B.y - q4d * N.y

    assert A2.ang_vel_in(N) == q4d * A2.y
    assert A2.ang_vel_in(A) == q4d * A2.y - q1d * N.z
    assert A2.ang_vel_in(B) == q4d * N.y - q1d * A.z - q2d * A.x
    assert A2.ang_vel_in(C) == q4d * N.y - q1d * A.z - q2d * A.x - q3d * B.y
    assert A2.ang_vel_in(A2) == 0

    C.set_ang_vel(N, u1 * C.x + u2 * C.y + u3 * C.z)
    assert C.ang_vel_in(N) == (u1) * C.x + (u2) * C.y + (u3) * C.z
    assert N.ang_vel_in(C) == (-u1) * C.x + (-u2) * C.y + (-u3) * C.z
    assert C.ang_vel_in(D) == (u1) * C.x + (u2) * C.y + (u3) * C.z + (-q4d) * D.y
    assert D.ang_vel_in(C) == (-u1) * C.x + (-u2) * C.y + (-u3) * C.z + (q4d) * D.y

    q0 = dynamicsymbols("q0")
    q0d = dynamicsymbols("q0", 1)
    E = N.orientnew("E", "Quaternion", (q0, q1, q2, q3))
    assert E.ang_vel_in(N) == (
        2 * (q1d * q0 + q2d * q3 - q3d * q2 - q0d * q1) * E.x
        + 2 * (q2d * q0 + q3d * q1 - q1d * q3 - q0d * q2) * E.y
        + 2 * (q3d * q0 + q1d * q2 - q2d * q1 - q0d * q3) * E.z
    )

    F = N.orientnew("F", "Body", (q1, q2, q3), "313")
    assert F.ang_vel_in(N) == (
        (sin(q2) * sin(q3) * q1d + cos(q3) * q2d) * F.x
        + (sin(q2) * cos(q3) * q1d - sin(q3) * q2d) * F.y
        + (cos(q2) * q1d + q3d) * F.z
    )
    G = N.orientnew("G", "Axis", (q1, N.x + N.y))
    assert G.ang_vel_in(N) == q1d * (N.x + N.y).normalize()
    assert N.ang_vel_in(G) == -q1d * (N.x + N.y).normalize()

# 需要导入模块: from sympy.physics.mechanics import ReferenceFrame [as 别名]
# 或者: from sympy.physics.mechanics.ReferenceFrame import ang_vel_in [as 别名]
b_length = symbols('l_b')
C.set_pos(B, b_length*b_frame.y)
D = Point('D')
c_length = symbols('l_c')
D.set_pos(C, c_length*c_frame.y)

#Sets up the angular velocities
omega_a, omega_b, omega_c = dynamicsymbols('omega_a, omega_b, omega_c')
#Relates angular velocity values to the angular positions theta1 and theta2
kinematic_differential_equations = [omega_a - theta_a.diff(),
                                    omega_b - theta_b.diff(),
                                    omega_c - theta_c.diff()]

#Sets up the rotational axes of the angular velocities
a_frame.set_ang_vel(inertial_frame, omega_a*inertial_frame.z)
a_frame.ang_vel_in(inertial_frame)
b_frame.set_ang_vel(a_frame, omega_b*inertial_frame.z)
b_frame.ang_vel_in(inertial_frame)
c_frame.set_ang_vel(b_frame, omega_c*inertial_frame.z)
c_frame.ang_vel_in(inertial_frame)

#Sets up the linear velocities of the points on the linkages
A.set_vel(inertial_frame, 0)
B.v2pt_theory(A, inertial_frame, a_frame)
B.vel(inertial_frame)
C.v2pt_theory(B, inertial_frame, b_frame)
C.vel(inertial_frame)
D.v2pt_theory(C, inertial_frame, c_frame)
D.vel(inertial_frame)

#Sets up the masses of the linkages

# 需要导入模块: from sympy.physics.mechanics import ReferenceFrame [as 别名]
# 或者: from sympy.physics.mechanics.ReferenceFrame import ang_vel_in [as 别名]
one_length = symbols('l_1')
two = Point('2')
two.set_pos(one, one_length*one_frame.y)
three = Point('3')
two_length = symbols('l_2')
three.set_pos(two, two_length*two_frame.y)

#Sets up the angular velocities
omega1, omega2 = dynamicsymbols('omega1, omega2')
#Relates angular velocity values to the angular positions theta1 and theta2
kinematic_differential_equations = [omega1 - theta1.diff(),
                                    omega2 - theta2.diff()]

#Sets up the rotational axes of the angular velocities
one_frame.set_ang_vel(inertial_frame, omega1*inertial_frame.z)
one_frame.ang_vel_in(inertial_frame)
two_frame.set_ang_vel(one_frame, omega2*inertial_frame.z)
two_frame.ang_vel_in(inertial_frame)

#Sets up the linear velocities of the points on the linkages
#one.set_vel(inertial_frame, 0)
two.v2pt_theory(one, inertial_frame, one_frame)
two.vel(inertial_frame)
three.v2pt_theory(two, inertial_frame, two_frame)
three.vel(inertial_frame)

#Sets up the masses of the linkages
one_mass, two_mass = symbols('m_1, m_2')

#Defines the linkages as particles
twoP = Particle('twoP', two, one_mass)

# 需要导入模块: from sympy.physics.mechanics import ReferenceFrame [as 别名]
# 或者: from sympy.physics.mechanics.ReferenceFrame import ang_vel_in [as 别名]
pO = Point('O')
pS_star = pO.locatenew('S*', b*A.y)
pS_hat = pS_star.locatenew('S^', -r*B.y) # S^ touches the cone
pS1 = pS_star.locatenew('S1', -r*A.z) # S1 touches horizontal wall of the race
pS2 = pS_star.locatenew('S2', r*A.y) # S2 touches vertical wall of the race

pO.set_vel(R, 0)
pS_star.v2pt_theory(pO, R, A)
pS1.v2pt_theory(pS_star, R, S)
pS2.v2pt_theory(pS_star, R, S)

# Since S is rolling against R, v_S1_R = 0, v_S2_R = 0.
vc = [dot(p.vel(R), basis) for p in [pS1, pS2] for basis in R]

pO.set_vel(C, 0)
pS_star.v2pt_theory(pO, C, A)
pS_hat.v2pt_theory(pS_star, C, S)

# Since S is rolling against C, v_S^_C = 0.
# Cone has only angular velocity in R.z direction.
vc += [dot(pS_hat.vel(C), basis).subs(vc_map) for basis in A]
vc += [dot(C.ang_vel_in(R), basis) for basis in [R.x, R.y]]
vc_map = solve(vc, u)

# Pure rolling between S and C, dot(ω_C_S, B.y) = 0.
b_val = solve([dot(C.ang_vel_in(S), B.y).subs(vc_map).simplify()], b)[0][0]
print('b = {0}'.format(msprint(collect(cancel(expand_trig(b_val)), r))))

b_expected = r*(1 + sin(theta))/(cos(theta) - sin(theta))
assert trigsimp(b_val - b_expected) == 0

# 需要导入模块: from sympy.physics.mechanics import ReferenceFrame [as 别名]
# 或者: from sympy.physics.mechanics.ReferenceFrame import ang_vel_in [as 别名]
# Since S is rolling against R, v_S1_R = 0, v_S2_R = 0.
pO.set_vel(R, 0)
pS_star.v2pt_theory(pO, R, A)
pS1.v2pt_theory(pS_star, R, S)
pS2.v2pt_theory(pS_star, R, S)
vc = [dot(p.vel(R), basis) for p in [pS1, pS2] for basis in R]

# Since S is rolling against C, v_S^_C = 0.
pO.set_vel(C, 0)
pS_star.v2pt_theory(pO, C, A)
pS_hat.v2pt_theory(pS_star, C, S)
vc += [dot(pS_hat.vel(C), basis) for basis in A]

# Cone has only angular velocity ω in R.z direction.
vc += [dot(C.ang_vel_in(R), basis) for basis in [R.x, R.y]]
vc += [omega - dot(C.ang_vel_in(R), R.z)]

vc_map = solve(vc, u)

# cone rigidbody
I_C = inertia(A, I11, I22, J)
rbC = RigidBody('rbC', pO, C, M, (I_C, pO))
# sphere rigidbody
I_S = inertia(A, 2*m*r**2/5, 2*m*r**2/5, 2*m*r**2/5)
rbS = RigidBody('rbS', pS_star, S, m, (I_S, pS_star))

# kinetic energy
K = radsimp(expand((rbC.kinetic_energy(R) +
                    4*rbS.kinetic_energy(R)).subs(vc_map)))
print('K = {0}'.format(msprint(collect(K, omega**2/2))))

# 需要导入模块: from sympy.physics.mechanics import ReferenceFrame [as 别名]
# 或者: from sympy.physics.mechanics.ReferenceFrame import ang_vel_in [as 别名]
# Define kinematical differential equations
# =========================================

omega1_dp1, omega2_dp1, omega1_dp2, omega2_dp2 = dynamicsymbols("omega_1dp1, omega_2dp1, omega_1dp2, omega_2dp2")

time = symbols("t")

kinematical_differential_equations_dp1 = [omega1_dp1 - theta1_dp1.diff(time), omega2_dp1 - theta2_dp2.diff(time)]

kinematical_differential_equations_dp2 = [omega1_dp2 - theta1_dp2.diff(time), omega2_dp2 - theta2_dp2.diff(time)]

# Angular Velocities
# ==================

one_frame_dp1.set_ang_vel(inertial_frame_dp1, omega1_dp1 * inertial_frame_dp1.z)
one_frame_dp1.ang_vel_in(inertial_frame_dp1)

one_frame_dp2.set_ang_vel(inertial_frame_dp2, omega1_dp2 * inertial_frame_dp2.z)
one_frame_dp2.ang_vel_in(inertial_frame_dp2)

two_frame_dp1.set_ang_vel(one_frame_dp1, omega2_dp1 * one_frame_dp1.z - omega1_dp2 * one_frame_dp1.z)
two_frame_dp1.ang_vel_in(inertial_frame_dp1)
two_frame_dp2.set_ang_vel(one_frame_dp2, omega2_dp2 * one_frame_dp2.z)
two_frame_dp2.ang_vel_in(inertial_frame_dp2)

# Linear Velocities
# =================

one_dp1.set_vel(inertial_frame_dp1, 0)
one_dp2.set_vel(inertial_frame_dp2, two_dp1.vel)