Python Point.v2pt_theory方法代码示例

# 需要导入模块: from sympy.physics.mechanics import Point [as 别名]
# 或者: from sympy.physics.mechanics.Point import v2pt_theory [as 别名]
kinematical_differential_equations = [omega1 - theta1.diff(time), omega2 - theta2.diff(time)]

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

l_leg_frame.set_ang_vel(inertial_frame, omega1 * inertial_frame.z)

body_frame.set_ang_vel(l_leg_frame, omega2 * l_leg_frame.z)

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

l_ankle.set_vel(inertial_frame, 0)

l_leg_mass_center.v2pt_theory(l_ankle, inertial_frame, l_leg_frame)

l_hip.v2pt_theory(l_ankle, inertial_frame, l_leg_frame)

r_hip.v2pt_theory(l_hip, inertial_frame, body_frame)

body_mass_center.v2pt_theory(l_hip, inertial_frame, body_frame)

r_leg_mass_center.v2pt_theory(r_hip, inertial_frame, body_frame)

# Mass
# ====

l_leg_mass, body_mass, r_leg_mass = symbols("m_L, m_B, m_R")

# Inertia

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

# 需要导入模块: from sympy.physics.mechanics import Point [as 别名]
# 或者: from sympy.physics.mechanics.Point import v2pt_theory [as 别名]
                                      omega_c - theta_c.diff(),
                                      omega_go - theta_]

# Angular Velocities
# ==================
a_frame.set_ang_vel(inertial_frame, omega_a*inertial_frame.z)
b_frame.set_ang_vel(a_frame, omega_b*inertial_frame.z)
c_frame.set_ang_vel(b_frame, omega_c*inertial_frame.z)

go_frame.set_ang_vel(inertial_frame, omega_go*inertial_frame.z)
bco_frame.set_ang_vel(b_frame, omega_bco*inertial_frame.z)

# Linear Velocities
# =================
A.set_vel(inertial_frame, 0)
B.v2pt_theory(A, inertial_frame, a_frame)
C.v2pt_theory(B, inertial_frame, b_frame)
D.v2pt_theory(C, inertial_frame, c_frame)
com_global.v2pt_theory(A, inertial_frame, go_frame)
com_bc.v2pt_theory(B, inertial_frame, bco_frame)

# Mass
# ====

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)
goP = Particle('goP', com_global, a_mass+b_mass+c_mass)

# 需要导入模块: from sympy.physics.mechanics import Point [as 别名]
# 或者: from sympy.physics.mechanics.Point import v2pt_theory [as 别名]
S = Point('S')
s_length = symbols('l_s')
T = Point('T')
T.set_pos(S, s_length*s_frame.y)

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

#Sets up the rotational axes of the angular velocities
s_frame.set_ang_vel(inertial_frame, omega_s*inertial_frame.z)

#Sets up the linear velocities of the points on the linkages
S.set_vel(inertial_frame, 0)
T.v2pt_theory(S, inertial_frame, s_frame)

#Sets up the masses of the linkages
s_mass = symbols('m_s')

#Defines the linkages as particles
tP = Particle('tP', T, s_mass)

#Sets up gravity information and assigns gravity to act on mass centers
g = symbols('g')
t_grav_force_vector = -1*s_mass*g*inertial_frame.y
t_grav_force = (T, t_grav_force_vector)

#Sets up joint torques
s_torque = dynamicsymbols('T_s')
s_torque_vector = s_torque*inertial_frame.z

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

# 需要导入模块: from sympy.physics.mechanics import Point [as 别名]
# 或者: from sympy.physics.mechanics.Point import v2pt_theory [as 别名]
                                      omega2 - theta2.diff(time)]

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

lower_leg_frame.set_ang_vel(inertial_frame, omega1 * inertial_frame.z)

upper_leg_frame.set_ang_vel(lower_leg_frame, omega2 * lower_leg_frame.z)


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

ankle.set_vel(inertial_frame, 0)

lower_leg_mass_center.v2pt_theory(ankle, inertial_frame, lower_leg_frame)

knee.v2pt_theory(ankle, inertial_frame, lower_leg_frame)

upper_leg_mass_center.v2pt_theory(knee, inertial_frame, upper_leg_frame)

# Mass
# ====

lower_leg_mass, upper_leg_mass = symbols('m_L, m_U')

# Inertia
# =======

lower_leg_inertia, upper_leg_inertia, torso_inertia = symbols('I_Lz, I_Uz, I_Tz')

# 需要导入模块: from sympy.physics.mechanics import Point [as 别名]
# 或者: from sympy.physics.mechanics.Point import v2pt_theory [as 别名]
time = symbols('t')

kinematical_differential_equations = [omega1 - theta1.diff(time),
                                      omega2 - theta2.diff(time),
                                      omega3 - theta3.diff(time)]

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

lower_leg_frame.set_ang_vel(inertial_frame, omega1 * inertial_frame.z)

upper_leg_frame.set_ang_vel(lower_leg_frame, omega2 * lower_leg_frame.z)

torso_frame.set_ang_vel(upper_leg_frame, omega3 * upper_leg_frame.z)

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

ankle.set_vel(inertial_frame, 0)

lower_leg_mass_center.v2pt_theory(ankle, inertial_frame, lower_leg_frame)

knee.v2pt_theory(ankle, inertial_frame, lower_leg_frame)

upper_leg_mass_center.v2pt_theory(knee, inertial_frame, upper_leg_frame)

hip.v2pt_theory(knee, inertial_frame, upper_leg_frame)

torso_mass_center.v2pt_theory(hip, inertial_frame, torso_frame)

# 需要导入模块: from sympy.physics.mechanics import Point [as 别名]
# 或者: from sympy.physics.mechanics.Point import v2pt_theory [as 别名]
hip_frame.set_ang_vel(hip_frame,   omega1     * inertial_frame.z)
#hip_frame.ang_vel_in(inertial_frame)

upper_leg_right_frame.set_ang_vel(upper_leg_right_frame, omega2     * inertial_frame.z)
#upper_leg_right_frame.ang_vel_in(inertial_frame)

lower_leg_right_frame.set_ang_vel(lower_leg_right_frame, omega3     * inertial_frame.z)
#lower_leg_right_frame.ang_vel_in(inertial_frame)
#%% Linear Velocities
print("Defining linear velocities")
origin.set_vel(inertial_frame, 0)

ankle_left.set_vel(inertial_frame, 0)

knee_left.v2pt_theory(ankle_left, inertial_frame, lower_leg_left_frame)
#knee_left.vel(inertial_frame)

hip_left.v2pt_theory(knee_left, inertial_frame, upper_leg_left_frame)
#hip_left.vel(inertial_frame)

hip_center.v2pt_theory(hip_left, inertial_frame, hip_frame)
#hip_center.vel(inertial_frame)

hip_mass_center.v2pt_theory(hip_center, inertial_frame, hip_frame)
#hip_mass_center.vel(inertial_frame)

hip_right.v2pt_theory(hip_center, inertial_frame, hip_frame)
#hip_right.vel(inertial_frame)

knee_right.v2pt_theory(hip_right, inertial_frame, upper_leg_right_frame)

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

# 需要导入模块: from sympy.physics.mechanics import Point [as 别名]
# 或者: from sympy.physics.mechanics.Point import v2pt_theory [as 别名]
# ==================

l_leg_frame.set_ang_vel(inertial_frame, omega1 * inertial_frame.z)

crotch_frame.set_ang_vel(l_leg_frame, omega2 * l_leg_frame.z)

body_frame.set_ang_vel(crotch_frame, omega4 * crotch_frame.z)

r_leg_frame.set_ang_vel(crotch_frame, omega3 * crotch_frame.z)

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

l_ankle.set_vel(inertial_frame, 0)

l_leg_mass_center.v2pt_theory(l_ankle, inertial_frame, l_leg_frame)

l_hip.v2pt_theory(l_ankle, inertial_frame, l_leg_frame)

crotch_mass_center.v2pt_theory(l_hip, inertial_frame, crotch_frame)

waist.v2pt_theory(l_hip, inertial_frame, crotch_frame)

body_mass_center.v2pt_theory(waist, inertial_frame, body_frame)

r_hip.v2pt_theory(l_hip, inertial_frame, crotch_frame)

r_leg_mass_center.v2pt_theory(r_hip, inertial_frame, r_leg_frame)

# Mass
# ====

# 需要导入模块: from sympy.physics.mechanics import Point [as 别名]
# 或者: from sympy.physics.mechanics.Point import v2pt_theory [as 别名]
                                      omega2 - theta2.diff(time)]

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

# 需要导入模块: from sympy.physics.mechanics import Point [as 别名]
# 或者: from sympy.physics.mechanics.Point import v2pt_theory [as 别名]
a_length = symbols('l_a')
B = Point('B')
B.set_pos(A, a_length*a_frame.y)

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

#Sets up the rotational axes of the angular velocities
a_frame.set_ang_vel(inertial_frame, omega_base*inertial_frame.z + omega_a*inertial_frame.z)

#Sets up the linear velocities of the points on the linkages
base_vel_x, base_vel_y = dynamicsymbols('v_bx, v_by')
A.set_vel(inertial_frame, base_vel_x*base_frame.x + base_vel_y*base_frame.y)
B.v2pt_theory(A, inertial_frame, a_frame)

#Sets up the masses of the linkages
a_mass = symbols('m_a')

#Defines the linkages as particles
bP = Particle('bP', B, a_mass)

#Sets up gravity information and assigns gravity to act on mass centers
g = symbols('g')
b_grav_force_vector = -1*a_mass*g*inertial_frame.y
b_grav_force = (B, b_grav_force_vector)

#Sets up joint torques
a_torque = dynamicsymbols('T_a')
a_torque_vector = a_torque*inertial_frame.z

# 需要导入模块: from sympy.physics.mechanics import Point [as 别名]
# 或者: from sympy.physics.mechanics.Point import v2pt_theory [as 别名]
from sympy.physics.mechanics import dot, dynamicsymbols, inertia, msprint


m, B11, B22, B33, B12, B23, B31 = symbols('m B11 B22 B33 B12 B23 B31')
q1, q2, q3, q4, q5, q6 = dynamicsymbols('q1:7')

# reference frames
A = ReferenceFrame('A')
B = A.orientnew('B', 'body', [q4, q5, q6], 'xyz')
omega = B.ang_vel_in(A)

# points B*, O
pB_star = Point('B*')
pO = pB_star.locatenew('O', q1*B.x + q2*B.y + q3*B.z)
pO.set_vel(A, 0)
pO.set_vel(B, 0)
pB_star.v2pt_theory(pO, A, B)

# rigidbody B
I_B_O = inertia(B, B11, B22, B33, B12, B23, B31)
rbB = RigidBody('rbB', pB_star, B, m, (I_B_O, pO))

# kinetic energy
K = rbB.kinetic_energy(A)
print('K = {0}'.format(msprint(trigsimp(K))))

K_expected = dot(dot(omega, I_B_O), omega)/2
print('K_expected = 1/2*omega*I*omega = {0}'.format(
        msprint(trigsimp(K_expected))))
assert expand(expand_trig(K - K_expected)) == 0