Python Point.vel方法代码示例

# 需要导入模块: from sympy.physics.mechanics import Point [as 别名]
# 或者: from sympy.physics.mechanics.Point import vel [as 别名]
def test_point_funcs():
    q, q2 = dynamicsymbols('q q2')
    qd, q2d = dynamicsymbols('q q2', 1)
    qdd, q2dd = dynamicsymbols('q q2', 2)
    N = ReferenceFrame('N')
    B = ReferenceFrame('B')
    B.set_ang_vel(N, 5 * B.y)
    O = Point('O')
    P = O.locatenew('P', q * B.x)
    assert P.pos_from(O) == q * B.x
    P.set_vel(B, qd * B.x + q2d * B.y)
    assert P.vel(B) == qd * B.x + q2d * B.y
    O.set_vel(N, 0)
    assert O.vel(N) == 0
    assert P.a1pt_theory(O, N, B) == ((-25 * q + qdd) * B.x + (q2dd) * B.y +
                               (-10 * qd) * B.z)

    B = N.orientnew('B', 'Axis', [q, N.z])
    O = Point('O')
    P = O.locatenew('P', 10 * B.x)
    O.set_vel(N, 5 * N.x)
    assert O.vel(N) == 5 * N.x
    assert P.a2pt_theory(O, N, B) == (-10 * qd**2) * B.x + (10 * qdd) * B.y

    B.set_ang_vel(N, 5 * B.y)
    O = Point('O')
    P = O.locatenew('P', q * B.x)
    P.set_vel(B, qd * B.x + q2d * B.y)
    O.set_vel(N, 0)
    assert P.v1pt_theory(O, N, B) == qd * B.x + q2d * B.y - 5 * q * B.z

# 需要导入模块: from sympy.physics.mechanics import Point [as 别名]
# 或者: from sympy.physics.mechanics.Point import vel [as 别名]
def test_partial_velocity():
    q1, q2, q3, u1, u2, u3 = dynamicsymbols("q1 q2 q3 u1 u2 u3")
    u4, u5 = dynamicsymbols("u4, u5")
    r = symbols("r")

    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)

    C = Point("C")
    C.set_vel(N, u4 * L.x + u5 * (Y.z ^ L.x))
    Dmc = C.locatenew("Dmc", r * L.z)
    Dmc.v2pt_theory(C, N, R)

    vel_list = [Dmc.vel(N), C.vel(N), R.ang_vel_in(N)]
    u_list = [u1, u2, u3, u4, u5]
    assert partial_velocity(vel_list, u_list) == [
        [-r * L.y, 0, L.x],
        [r * L.x, 0, L.y],
        [0, 0, L.z],
        [L.x, L.x, 0],
        [cos(q2) * L.y - sin(q2) * L.z, cos(q2) * L.y - sin(q2) * L.z, 0],
    ]

# 需要导入模块: from sympy.physics.mechanics import Point [as 别名]
# 或者: from sympy.physics.mechanics.Point import vel [as 别名]
def test_partial_velocity():
    q1, q2, q3, u1, u2, u3  = dynamicsymbols('q1 q2 q3 u1 u2 u3')
    u4, u5 = dynamicsymbols('u4, u5')
    r = symbols('r')

    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)

    C = Point('C')
    C.set_vel(N, u4 * L.x + u5 * (Y.z ^ L.x))
    Dmc = C.locatenew('Dmc', r * L.z)
    Dmc.v2pt_theory(C, N, R)

    vel_list = [Dmc.vel(N), C.vel(N), R.ang_vel_in(N)]
    u_list = [u1, u2, u3, u4, u5]
    assert (partial_velocity(vel_list, u_list) == [[- r*L.y, 0, L.x],
            [r*L.x, 0, L.y], [0, 0, L.z], [L.x, L.x, 0],
                [cos(q2)*L.y - sin(q2)*L.z, cos(q2)*L.y - sin(q2)*L.z, 0]])

# 需要导入模块: from sympy.physics.mechanics import Point [as 别名]
# 或者: from sympy.physics.mechanics.Point import vel [as 别名]
# Rattleback ground contact point
P = Point('P')
P.set_vel(N, ua[0]*Y.x + ua[1]*Y.y + ua[2]*Y.z)

# Rattleback paraboloid -- parameterize coordinates of contact point by the
# roll and pitch angles, and the geometry
# TODO: FIXME!!!
# f(x,y,z) = a*x**2 + b*y**2 + z - c
mu = [dot(rk, Y.z) for rk in R]
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]

# 需要导入模块: from sympy.physics.mechanics import Point [as 别名]
# 或者: from sympy.physics.mechanics.Point import vel [as 别名]
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, mannual 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 = {udi : 0 for udi in ud}
    ud_zero = dict(zip(ud, [0.]*len(ud)))
    ua = dynamicsymbols('ua:3')
    #ua_zero = {uai : 0 for uai in ua}
    ua_zero = dict(zip(ua, [0.]*len(ua)))

    # 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)

    # 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])
#.........这里部分代码省略.........

# 需要导入模块: from sympy.physics.mechanics import Point [as 别名]
# 或者: from sympy.physics.mechanics.Point import vel [as 别名]
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)
r_leg_mass_center.v2pt_theory(r_hip, inertial_frame, r_leg_frame)
r_leg_mass_center.vel(inertial_frame)

#Sets up the masses of the linkages
l_leg_mass, body_mass, r_leg_mass = symbols('m_LL, m_B, m_RL')

#Sets up the rotational inertia of the linkages
l_leg_inertia, body_inertia, r_leg_inertia = symbols('I_LLz, I_Bz, I_RLz')

# 需要导入模块: from sympy.physics.mechanics import Point [as 别名]
# 或者: from sympy.physics.mechanics.Point import vel [as 别名]
#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
leg_frame.set_ang_vel(inertial_frame, omega1*inertial_frame.z)
leg_frame.ang_vel_in(inertial_frame)
body_frame.set_ang_vel(leg_frame, omega2*inertial_frame.z)
body_frame.ang_vel_in(inertial_frame)

#Sets up the linear velocities of the points on the linkages
ankle.set_vel(inertial_frame, 0)
waist.v2pt_theory(ankle, inertial_frame, leg_frame)
waist.vel(inertial_frame)
body.v2pt_theory(waist, inertial_frame, body_frame)
body.vel(inertial_frame)

#Sets up the masses of the linkages
leg_mass, body_mass = symbols('m_L, m_B')

#Defines the linkages as particles
waistP = Particle('waistP', waist, leg_mass)
bodyP = Particle('bodyP', body, body_mass)

#Sets up gravity information and assigns gravity to act on mass centers
g = symbols('g')
leg_grav_force_vector = -1*leg_mass*g*inertial_frame.y
leg_grav_force = (waist, leg_grav_force_vector)
body_grav_force_vector = -1*body_mass*g*inertial_frame.y

# 需要导入模块: from sympy.physics.mechanics import Point [as 别名]
# 或者: from sympy.physics.mechanics.Point import vel [as 别名]
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
a_mass, b_mass, c_mass = symbols('m_a, m_b, m_c')

#Defines the linkages as particles
bP = Particle('bP', B, a_mass)
cP = Particle('cP', C, b_mass)
dP = Particle('dP', D, c_mass)

#Sets up gravity information and assigns gravity to act on mass centers
g = symbols('g')

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

one_mass_center_dp1.v2pt_theory(one_dp1, inertial_frame_dp1, one_frame_dp1)
one_mass_center_dp1.vel(inertial_frame_dp1)
one_mass_center_dp2.v2pt_theory(one_dp2, inertial_frame_dp2, one_frame_dp2)
one_mass_center_dp2.vel(inertial_frame_dp2)

two_dp1.v2pt_theory(one_dp1, inertial_frame_dp1, one_frame_dp1)
two_dp1.vel(inertial_frame_dp1)
two_dp2.v2pt_theory(one_dp2, inertial_frame_dp2, one_frame_dp2)
two_dp2.vel(inertial_frame_dp2)

two_mass_center_dp1.v2pt_theory(two_dp1, inertial_frame_dp1, two_frame_dp1)
two_mass_center_dp2.v2pt_theory(two_dp2, inertial_frame_dp2, two_frame_dp2)

# Mass
# ====

a_mass, b_mass, c_mass = symbols("m_a, m_b, m_c")

# 需要导入模块: from sympy.physics.mechanics import Point [as 别名]
# 或者: from sympy.physics.mechanics.Point import vel [as 别名]
# 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 * one_frame.z)
two_frame.ang_vel_in(inertial_frame)

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

one.set_vel(inertial_frame, 0)

one_mass_center.v2pt_theory(one, inertial_frame, one_frame)
one_mass_center.vel(inertial_frame)
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)