Python Point.set_pos方法代码示例

# 需要导入模块: from sympy.physics.mechanics import Point [as 别名]
# 或者: from sympy.physics.mechanics.Point import set_pos [as 别名]
def test_particle():
    m, m2, v1, v2, v3, r = symbols('m m2 v1 v2 v3 r')
    P = Point('P')
    P2 = Point('P2')
    p = Particle('pa', P, m)
    assert p.mass == m
    assert p.point == P
    # Test the mass setter
    p.mass = m2
    assert p.mass == m2
    # Test the point setter
    p.point = P2
    assert p.point == P2
    # Test the linear momentum function
    N = ReferenceFrame('N')
    O = Point('O')
    P2.set_pos(O, r * N.y)
    P2.set_vel(N, v1 * N.x)
    assert p.linearmomentum(N) == m2 * v1 * N.x
    assert p.angularmomentum(O, N) == -m2 * r *v1 * N.z
    P2.set_vel(N, v2 * N.y)
    assert p.linearmomentum(N) == m2 * v2 * N.y
    assert p.angularmomentum(O, N) == 0
    P2.set_vel(N, v3 * N.z)
    assert p.linearmomentum(N) == m2 * v3 * N.z
    assert p.angularmomentum(O, N) == m2 * r * v3 * N.x
    P2.set_vel(N, v1 * N.x + v2 * N.y + v3 * N.z)
    assert p.linearmomentum(N) == m2 * (v1 * N.x + v2 * N.y + v3 * N.z)
    assert p.angularmomentum(O, N) == m2 * r * (v3 * N.x - v1 * N.z)

# 需要导入模块: from sympy.physics.mechanics import Point [as 别名]
# 或者: from sympy.physics.mechanics.Point import set_pos [as 别名]
def test_particle():
    m, m2, v1, v2, v3, r, g, h = symbols('m m2 v1 v2 v3 r g h')
    P = Point('P')
    P2 = Point('P2')
    p = Particle('pa', P, m)
    assert p.mass == m
    assert p.point == P
    # Test the mass setter
    p.mass = m2
    assert p.mass == m2
    # Test the point setter
    p.point = P2
    assert p.point == P2
    # Test the linear momentum function
    N = ReferenceFrame('N')
    O = Point('O')
    P2.set_pos(O, r * N.y)
    P2.set_vel(N, v1 * N.x)
    assert p.linear_momentum(N) == m2 * v1 * N.x
    assert p.angular_momentum(O, N) == -m2 * r *v1 * N.z
    P2.set_vel(N, v2 * N.y)
    assert p.linear_momentum(N) == m2 * v2 * N.y
    assert p.angular_momentum(O, N) == 0
    P2.set_vel(N, v3 * N.z)
    assert p.linear_momentum(N) == m2 * v3 * N.z
    assert p.angular_momentum(O, N) == m2 * r * v3 * N.x
    P2.set_vel(N, v1 * N.x + v2 * N.y + v3 * N.z)
    assert p.linear_momentum(N) == m2 * (v1 * N.x + v2 * N.y + v3 * N.z)
    assert p.angular_momentum(O, N) == m2 * r * (v3 * N.x - v1 * N.z)
    p.set_potential_energy(m * g * h)
    assert p.potential_energy == m * g * h
    # TODO make the result not be system-dependent
    assert p.kinetic_energy(
        N) in [m2*(v1**2 + v2**2 + v3**2)/2,
        m2 * v1**2 / 2 + m2 * v2**2 / 2 + m2 * v3**2 / 2]

# 需要导入模块: from sympy.physics.mechanics import Point [as 别名]
# 或者: from sympy.physics.mechanics.Point import set_pos [as 别名]
def test_rigidbody2():
    M, v, r, omega = dynamicsymbols('M v r omega')
    N = ReferenceFrame('N')
    b = ReferenceFrame('b')
    b.set_ang_vel(N, omega * b.x)
    P = Point('P')
    I = outer (b.x, b.x)
    Inertia_tuple = (I, P)
    B = RigidBody('B', P, b, M, Inertia_tuple)
    P.set_vel(N, v * b.x)
    assert B.angularmomentum(P, N) == omega * b.x
    O = Point('O')
    O.set_vel(N, v * b.x)
    P.set_pos(O, r * b.y)
    assert B.angularmomentum(O, N) == omega * b.x - M*v*r*b.z

# 需要导入模块: from sympy.physics.mechanics import Point [as 别名]
# 或者: from sympy.physics.mechanics.Point import set_pos [as 别名]
def test_rigidbody2():
    M, v, r, omega, g, h = dynamicsymbols('M v r omega g h')
    N = ReferenceFrame('N')
    b = ReferenceFrame('b')
    b.set_ang_vel(N, omega * b.x)
    P = Point('P')
    I = outer(b.x, b.x)
    Inertia_tuple = (I, P)
    B = RigidBody('B', P, b, M, Inertia_tuple)
    P.set_vel(N, v * b.x)
    assert B.angular_momentum(P, N) == omega * b.x
    O = Point('O')
    O.set_vel(N, v * b.x)
    P.set_pos(O, r * b.y)
    assert B.angular_momentum(O, N) == omega * b.x - M*v*r*b.z
    B.set_potential_energy(M * g * h)
    assert B.potential_energy == M * g * h
    assert B.kinetic_energy(N) == (omega**2 + M * v**2) / 2

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

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

# Joints
# ------

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

# 需要导入模块: from sympy.physics.mechanics import Point [as 别名]
# 或者: from sympy.physics.mechanics.Point import set_pos [as 别名]
                            vol_C))
print('\nC* = {0}'.format(pCs.pos_from(pC_O)))
print('C* = {0}'.format(eval_vec(pCs.pos_from(pC_O))))

## FIND CENTER OF MASS OF D
vol_D = pi*a**3/4
pD_O = Point('D_O')
pDs = pD_O.locatenew('D*', (a*N.z + a/2*N.y +
                            (N.x - N.z)*(centroid_sector.subs(
                                     theta_pi_4) * sin(pi/4))))
print('\nD* = {0}'.format(pDs.pos_from(pD_O)))
print('D* = {0}'.format(eval_vec(pDs.pos_from(pD_O))))

## FIND CENTER OF MASS OF ASSEMBLY
pO = Point('O')
pA_O.set_pos(pO, 2*a*N.x - (a+b)*N.z)
pB_O.set_pos(pO, 2*a*N.x - a*N.z)
pC_O.set_pos(pO, -(a+b)*N.z)
pD_O.set_pos(pO, -a*N.z)

density_A = 7800
density_B = 17.00
density_C = 2700
density_D = 8400
mass_A = vol_A * density_A
mass_B = vol_B * density_B
mass_C = vol_C * density_C
mass_D = vol_D * density_D

pms = pO.locatenew('m*', ((pAs.pos_from(pO)*mass_A + pBs.pos_from(pO)*mass_B +
                           pCs.pos_from(pO)*mass_C + pDs.pos_from(pO)*mass_D) /

# 需要导入模块: from sympy.physics.mechanics import Point [as 别名]
# 或者: from sympy.physics.mechanics.Point import set_pos [as 别名]
leg_frame = ReferenceFrame('L')
body_frame = ReferenceFrame('B')

#Sets up symbols for joint angles
theta1, theta2 = dynamicsymbols('theta1, theta2')

#Orients the leg frame to the inertial frame by angle theta1
#and the body frame to to the leg frame by angle theta2
leg_frame.orient(inertial_frame, 'Axis', (theta1, inertial_frame.z))
body_frame.orient(leg_frame, 'Axis', (theta2, leg_frame.z))

#Sets up points for the joints and places them relative to each other
ankle = Point('A')
leg_length = symbols('l_L')
waist = Point('W')
waist.set_pos(ankle, leg_length*leg_frame.y)
body = Point('B')
body_length = symbols('l_B')
body.set_pos(waist, body_length*body_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
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)

# 需要导入模块: from sympy.physics.mechanics import Point [as 别名]
# 或者: from sympy.physics.mechanics.Point import set_pos [as 别名]
c_frame = ReferenceFrame('C')

#Sets up symbols for joint angles
theta_a, theta_b, theta_c = dynamicsymbols('theta_a, theta_b, theta_c')

#Orients the leg frame to the inertial frame by angle theta1
#and the body frame to to the leg frame by angle theta2
a_frame.orient(inertial_frame, 'Axis', (theta_a, inertial_frame.z))
b_frame.orient(a_frame, 'Axis', (theta_b, a_frame.z))
c_frame.orient(b_frame, 'Axis', (theta_c, b_frame.z))

#Sets up points for the joints and places them relative to each other
A = Point('A')
a_length = symbols('l_a')
B = Point('B')
B.set_pos(A, a_length*a_frame.y)
C = Point('C')
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

# 需要导入模块: from sympy.physics.mechanics import Point [as 别名]
# 或者: from sympy.physics.mechanics.Point import set_pos [as 别名]
#Sets up inertial frame as well as frames for each linkage
inertial_frame = ReferenceFrame('I')
s_frame = ReferenceFrame('S')

#Sets up symbols for joint angles
theta_s = dynamicsymbols('theta_s')

#Orients the leg frame to the inertial frame by angle theta1
#and the body frame to to the leg frame by angle theta2
s_frame.orient(inertial_frame, 'Axis', (theta_s, inertial_frame.z))

#Sets up points for the joints and places them relative to each other
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')

# 需要导入模块: from sympy.physics.mechanics import Point [as 别名]
# 或者: from sympy.physics.mechanics.Point import set_pos [as 别名]
two_frame_dp2.orient(one_frame_dp2, "Axis", (theta2_dp2, one_frame_dp2.z))

# Point Locations
# ===============

# Joints
# ------

one_length_dp1, one_length_dp2, two_length_dp2 = symbols("l_1dp1, l_1dp2, l_2dp2")

one_dp1 = Point("1_dp1")
one_dp2 = Point("1_dp2")

two_dp1 = Point("2_dp1")
two_dp2 = Point("2_dp2")
two_dp1.set_pos(one_dp1, one_length_dp1 * one_frame_dp1.y)
two_dp2.set_pos(one_dp2, one_length_dp2 * one_frame_dp2.y)

three_dp2 = Point("3_dp2")
three_dp2.set_pos(two_dp2, two_length_dp2 * two_frame_dp2.y)

# Center of mass locations
# ------------------------

one_com_xdp1, one_com_ydp1, two_com_xdp1, two_com_ydp1 = dynamicsymbols(
    "1_comx_dp1, 1_comy_dp1, 2_comx_dp1, 2_comy_dp1"
)

one_mass_center_dp1 = Point("1_odp1")
one_mass_center_dp1.set_pos(one_dp1, one_length_dp1 * one_frame_dp1.y)

# 需要导入模块: from sympy.physics.mechanics import Point [as 别名]
# 或者: from sympy.physics.mechanics.Point import set_pos [as 别名]
upper_leg_frame.orient(lower_leg_frame, 'Axis', (theta2, lower_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)

# Center of mass locations
# ------------------------

lower_leg_com_length, upper_leg_com_length = symbols('d_L, d_U')

lower_leg_mass_center = Point('L_o')
lower_leg_mass_center.set_pos(ankle, lower_leg_com_length * lower_leg_frame.y)

upper_leg_mass_center = Point('U_o')
upper_leg_mass_center.set_pos(knee, upper_leg_com_length * upper_leg_frame.y)


# Define kinematical differential equations
# =========================================

# 需要导入模块: from sympy.physics.mechanics import Point [as 别名]
# 或者: from sympy.physics.mechanics.Point import set_pos [as 别名]
two_frame.orient(one_frame, 'Axis', (theta2, one_frame.z))


# Point Locations
# ===============

# Joints
# ------

one_length = symbols('l_1')

one = Point('1')

two = Point('2')
two.set_pos(one, one_length * one_frame.y)

# Center of mass locations
# ------------------------

one_com_x, one_com_y, two_com_x, two_com_y = dynamicsymbols('1_comx, 1_comy, 2_comx, 2_comy')

one_mass_center = Point('1_o')
one_mass_center.set_pos(one, one_com_x*inertial_frame.x + one_com_y*inertial_frame.y)

two_mass_center = Point('2_o')
two_mass_center.set_pos(two, two_com_x*inertial_frame.x + two_com_y*inertial_frame.y)


# Define kinematical differential equations
# =========================================

# 需要导入模块: from sympy.physics.mechanics import Point [as 别名]
# 或者: from sympy.physics.mechanics.Point import set_pos [as 别名]
body_frame = ReferenceFrame("B")

body_frame.orient(crotch_frame, "Axis", (theta4, crotch_frame.z))

# Point Locations
# ===============

# Joints
# ------

l_leg_length, hip_width, body_height = symbols("l_L, h_W, b_H")

l_ankle = Point("LA")

l_hip = Point("LH")
l_hip.set_pos(l_ankle, l_leg_length * l_leg_frame.y)

r_hip = Point("RH")
r_hip.set_pos(l_hip, hip_width * crotch_frame.x)

body_middle = Point("B_m")
body_middle.set_pos(l_hip, (hip_width / 2) * crotch_frame.x)

waist = Point("W")
waist.set_pos(body_middle, body_height * crotch_frame.y)

# Center of mass locations
# ------------------------

l_leg_com_length, r_leg_com_length, crotch_com_height, body_com_height = symbols("d_L, d_R, d_C, d_B")

# 需要导入模块: from sympy.physics.mechanics import Point [as 别名]
# 或者: from sympy.physics.mechanics.Point import set_pos [as 别名]
# these can be arbitray points on each of you robots links
# later these points are visualized, so you can verify the correct build of your robot
origin = Point('Origin')
ankle_left = Point('AnkleLeft')
knee_left = Point('KneeLeft')
hip_left = Point('HipLeft')
hip_center = Point('HipCenter')
hip_right = Point('HipRight')
knee_right = Point('KneeRight')
ankle_right = Point('AnkleRight')

# here go the lengths of your robots links
lower_leg_length, upper_leg_length, hip_length = symbols('l1, l2, l3')

ankle_left.set_pos(origin, (0 * inertial_frame.y)+(0 * inertial_frame.x))
#ankle_left.pos_from(origin).express(inertial_frame).simplify()

knee_left.set_pos(ankle_left, lower_leg_length * lower_leg_left_frame.y)
#knee_left.pos_from(origin).express(inertial_frame).simplify()

hip_left.set_pos(knee_left, upper_leg_length * upper_leg_left_frame.y)
#hip_left.pos_from(origin).express(inertial_frame).simplify()

hip_center.set_pos(hip_left, hip_length/2 * hip_frame.x)
#hip_center.pos_from(origin).express(inertial_frame).simplify()

hip_right.set_pos(hip_center, hip_length/2 * hip_frame.x)
#hip_right.pos_from(origin).express(inertial_frame).simplify()

knee_right.set_pos(hip_right, upper_leg_length * -upper_leg_right_frame.y)