Python RigidBody.set_potential_energy方法代码示例

# 需要导入模块: from sympy.physics.mechanics import RigidBody [as 别名]
# 或者: from sympy.physics.mechanics.RigidBody import set_potential_energy [as 别名]
def test_rolling_disc():
    # Rolling Disc Example
    # Here the rolling disc is formed from the contact point up, removing the
    # need to introduce generalized speeds. Only 3 configuration and 3
    # speed variables are need to describe this system, along with the
    # disc's mass and radius, and the local gravity.
    q1, q2, q3 = dynamicsymbols('q1 q2 q3')
    q1d, q2d, q3d = dynamicsymbols('q1 q2 q3', 1)
    r, m, g = symbols('r m g')

    # The kinematics are formed by a series of simple rotations. Each simple
    # rotation creates a new frame, and the next rotation is defined by the new
    # frame's basis vectors. This example uses a 3-1-2 series of rotations, or
    # Z, X, Y series of rotations. Angular velocity for this is defined using
    # the second frame's basis (the lean frame).
    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])

    # This is the translational kinematics. We create a point with no velocity
    # in N; this is the contact point between the disc and ground. Next we form
    # the position vector from the contact point to the disc's center of mass.
    # Finally we form the velocity and acceleration of the disc.
    C = Point('C')
    C.set_vel(N, 0)
    Dmc = C.locatenew('Dmc', r * L.z)
    Dmc.v2pt_theory(C, N, R)

    # Forming the inertia dyadic.
    I = inertia(L, m / 4 * r**2, m / 2 * r**2, m / 4 * r**2)
    BodyD = RigidBody('BodyD', Dmc, R, m, (I, Dmc))

    # Finally we form the equations of motion, using the same steps we did
    # before. Supply the Lagrangian, the generalized speeds.
    BodyD.set_potential_energy(- m * g * r * cos(q2))
    Lag = Lagrangian(N, BodyD)
    q = [q1, q2, q3]
    q1 = Function('q1')
    q2 = Function('q2')
    q3 = Function('q3')
    l = LagrangesMethod(Lag, q)
    l.form_lagranges_equations()
    RHS = l.rhs()
    RHS.simplify()
    t = symbols('t')

    assert (l.mass_matrix[3:6] == [0, 5*m*r**2/4, 0])
    assert RHS[4].simplify() == (-8*g*sin(q2(t)) + 5*r*sin(2*q2(t)
        )*Derivative(q1(t), t)**2 + 12*r*cos(q2(t))*Derivative(q1(t), t
        )*Derivative(q3(t), t))/(10*r)
    assert RHS[5] == (-5*cos(q2(t))*Derivative(q1(t), t) + 6*tan(q2(t)
        )*Derivative(q3(t), t) + 4*Derivative(q1(t), t)/cos(q2(t))
        )*Derivative(q2(t), t)

# 需要导入模块: from sympy.physics.mechanics import RigidBody [as 别名]
# 或者: from sympy.physics.mechanics.RigidBody import set_potential_energy [as 别名]
def test_disc_on_an_incline_plane():
    # Disc rolling on an inclined plane
    # First the generalized coordinates are created. The mass center of the
    # disc is located from top vertex of the inclined plane by the generalized
    # coordinate 'y'. The orientation of the disc is defined by the angle
    # 'theta'. The mass of the disc is 'm' and its radius is 'R'. The length of
    # the inclined path is 'l', the angle of inclination is 'alpha'. 'g' is the
    # gravitational constant.
    y, theta = dynamicsymbols('y theta')
    yd, thetad = dynamicsymbols('y theta', 1)
    m, g, R, l, alpha = symbols('m g R l alpha')

    # Next, we create the inertial reference frame 'N'. A reference frame 'A'
    # is attached to the inclined plane. Finally a frame is created which is attached to the disk.
    N = ReferenceFrame('N')
    A = N.orientnew('A', 'Axis', [pi/2 - alpha, N.z])
    B = A.orientnew('B', 'Axis', [-theta, A.z])

    # Creating the disc 'D'; we create the point that represents the mass
    # center of the disc and set its velocity. The inertia dyadic of the disc
    # is created. Finally, we create the disc.
    Do = Point('Do')
    Do.set_vel(N, yd * A.x)
    I = m * R**2 / 2 * B.z | B.z
    D = RigidBody('D', Do, B, m, (I, Do))

    # To construct the Lagrangian, 'L', of the disc, we determine its kinetic
    # and potential energies, T and U, respectively. L is defined as the
    # difference between T and U.
    D.set_potential_energy(m * g * (l - y) * sin(alpha))
    L = Lagrangian(N, D)

    # We then create the list of generalized coordinates and constraint
    # equations. The constraint arises due to the disc rolling without slip on
    # on the inclined path. Also, the constraint is holonomic but we supply the
    # differentiated holonomic equation as the 'LagrangesMethod' class requires
    # that. We then invoke the 'LagrangesMethod' class and supply it the
    # necessary arguments and generate the equations of motion. The'rhs' method
    # solves for the q_double_dots (i.e. the second derivative with respect to
    # time  of the generalized coordinates and the lagrange multiplers.
    q = [y, theta]
    coneq = [yd - R * thetad]
    m = LagrangesMethod(L, q, coneq)
    m.form_lagranges_equations()
    rhs = m.rhs()
    rhs.simplify()
    assert rhs[2] == 2*g*sin(alpha)/3

# 需要导入模块: from sympy.physics.mechanics import RigidBody [as 别名]
# 或者: from sympy.physics.mechanics.RigidBody import set_potential_energy [as 别名]
def test_potential_energy():
    m, M, l1, g, h, H = symbols('m M l1 g h H')
    omega = dynamicsymbols('omega')
    N = ReferenceFrame('N')
    O = Point('O')
    O.set_vel(N, 0 * N.x)
    Ac = O.locatenew('Ac', l1 * N.x)
    P = Ac.locatenew('P', l1 * N.x)
    a = ReferenceFrame('a')
    a.set_ang_vel(N, omega * N.z)
    Ac.v2pt_theory(O, N, a)
    P.v2pt_theory(O, N, a)
    Pa = Particle('Pa', P, m)
    I = outer(N.z, N.z)
    A = RigidBody('A', Ac, a, M, (I, Ac))
    Pa.set_potential_energy(m * g * h)
    A.set_potential_energy(M * g * H)
    assert potential_energy(A, Pa) == m * g * h + M * g * H

# 需要导入模块: from sympy.physics.mechanics import RigidBody [as 别名]
# 或者: from sympy.physics.mechanics.RigidBody import set_potential_energy [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 RigidBody [as 别名]
# 或者: from sympy.physics.mechanics.RigidBody import set_potential_energy [as 别名]
def test_linearize_rolling_disc_lagrange():
    q1, q2, q3 = q = dynamicsymbols("q1 q2 q3")
    q1d, q2d, q3d = qd = dynamicsymbols("q1 q2 q3", 1)
    r, m, g = symbols("r m g")

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

    C = Point("C")
    C.set_vel(N, 0)
    Dmc = C.locatenew("Dmc", r * L.z)
    Dmc.v2pt_theory(C, N, R)

    I = inertia(L, m / 4 * r ** 2, m / 2 * r ** 2, m / 4 * r ** 2)
    BodyD = RigidBody("BodyD", Dmc, R, m, (I, Dmc))
    BodyD.set_potential_energy(-m * g * r * cos(q2))

    Lag = Lagrangian(N, BodyD)
    l = LagrangesMethod(Lag, q)
    l.form_lagranges_equations()

    # Linearize about steady-state upright rolling
    op_point = {q1: 0, q2: 0, q3: 0, q1d: 0, q2d: 0, q1d.diff(): 0, q2d.diff(): 0, q3d.diff(): 0}
    A = l.linearize(q_ind=q, qd_ind=qd, op_point=op_point, A_and_B=True)[0]
    sol = Matrix(
        [
            [0, 0, 0, 1, 0, 0],
            [0, 0, 0, 0, 1, 0],
            [0, 0, 0, 0, 0, 1],
            [0, 0, 0, 0, -6 * q3d, 0],
            [0, -4 * g / (5 * r), 0, 6 * q3d / 5, 0, 0],
            [0, 0, 0, 0, 0, 0],
        ]
    )

    assert A == sol

# 需要导入模块: from sympy.physics.mechanics import RigidBody [as 别名]
# 或者: from sympy.physics.mechanics.RigidBody import set_potential_energy [as 别名]
    f: 2,
    v0: 20}

N = ReferenceFrame('N')
B = N.orientnew('B', 'axis', [q3, N.z])


O = Point('O')
S = O.locatenew('S', q1*N.x + q2*N.y)
S.set_vel(N, S.pos_from(O).dt(N))

#Is = m/12*(l**2 + w**2)
Is = symbols('Is')
I = inertia(B, 0, 0, Is, 0, 0, 0)
rb = RigidBody('rb', S, B, m, (I, S))
rb.set_potential_energy(0)


L = Lagrangian(N, rb)
lm = LagrangesMethod(
    L, q, nonhol_coneqs = [q1d*sin(q3) - q2d*cos(q3) + l/2*q3d])
lm.form_lagranges_equations()
rhs = lm.rhs()
print('{} = {}'.format(msprint(q1d.diff(t)),
    msprint(rhs[3].simplify())))
print('{} = {}'.format(msprint(q2d.diff(t)),
    msprint(rhs[4].simplify())))
print('{} = {}'.format(msprint(q3d.diff(t)),
    msprint(rhs[5].simplify())))
print('{} = {}'.format('λ', msprint(rhs[6].simplify())))

# 需要导入模块: from sympy.physics.mechanics import RigidBody [as 别名]
# 或者: from sympy.physics.mechanics.RigidBody import set_potential_energy [as 别名]
pC_star.v2pt_theory(pR, A, B)
pC_hat.v2pt_theory(pC_star, A, C)

# kinematic differential equations
# kde = [dot(C.ang_vel_in(A), x) - y for x, y in zip(B, u[:3])]
# kde += [x - y for x, y in zip(qd[3:], u[3:])]
# kde_map = solve(kde, qd)
kde = [x - y for x, y in zip(u, qd)]
kde_map = solve(kde, qd)
vc = map(lambda x: dot(pC_hat.vel(A), x), [A.x, A.y])
vc_map = solve(subs(vc, kde_map), [u4, u5])

# define disc rigidbody
IC = inertia(C, m * R ** 2 / 4, m * R ** 2 / 4, m * R ** 2 / 2)
rbC = RigidBody("rbC", pC_star, C, m, (IC, pC_star))
rbC.set_potential_energy(m * g * dot(pC_star.pos_from(pR), A.z))

# potential energy
V = rbC.potential_energy
print("V = {0}".format(msprint(V)))

# kinetic energy
K = trigsimp(rbC.kinetic_energy(A).subs(kde_map).subs(vc_map))
print("K = {0}".format(msprint(K)))

u_indep = [u1, u2, u3]
Fr = generalized_active_forces_K(K, q, u_indep, kde_map, vc_map)
# Fr + Fr* = 0 but the dynamical equations cannot be formulated by only
# kinetic energy as Fr = -Fr* for r = 1, ..., p
print("\ngeneralized active forces, Fr")
for i, x in enumerate(Fr, 1):

# 需要导入模块: from sympy.physics.mechanics import RigidBody [as 别名]
# 或者: from sympy.physics.mechanics.RigidBody import set_potential_energy [as 别名]
         l_leg_torque,
         body_torque,
         r_leg_torque]

bodies = [l_leg, body, r_leg]

fr, frstar = kane.kanes_equations(loads, bodies)

mass_matrix = kane.mass_matrix_full
forcing_vector = kane.forcing_full

# Set up kinetic and potential energies
# =====================================

ke_lleg = kinetic_energy(inertial_frame, l_leg)
l_leg.set_potential_energy(l_leg_mass*g*l_leg_com_length*cos(theta1))

ke_body = kinetic_energy(inertial_frame, body) - ke_lleg
body.set_potential_energy(body_mass*g*(l_leg_length*cos(theta1) + body_com_length*sin(theta1+theta2)+body_com_height*sin(theta1+theta2+1.57)))

ke_rleg = kinetic_energy(inertial_frame, r_leg) - ke_body - ke_lleg
r_leg.set_potential_energy(r_leg_mass*g*(l_leg_length*cos(theta1) + hip_width*sin(theta1+theta2)+-1*r_leg_com_length*cos(theta1+theta2+theta3)))


# List the symbolic arguments
# ===========================

# Constants
# ---------

constants = [l_leg_length,

# 需要导入模块: from sympy.physics.mechanics import RigidBody [as 别名]
# 或者: from sympy.physics.mechanics.RigidBody import set_potential_energy [as 别名]
from sympy.physics.vector import ReferenceFrame
from sympy.physics.mechanics import inertia, msprint
from sympy.physics.mechanics import Point, RigidBody
from sympy.physics.mechanics import Lagrangian, LagrangesMethod
from sympy import symbols

q = q1, q2, q3, q4, q5, q6 = dynamicsymbols("q1:7")
m, g, k, px, Ip = symbols("m g k px Ip")
N = ReferenceFrame("N")
B = N.orientnew("B", "body", [q4, q5, q6], "zyx")

A = Point("A")
S = A.locatenew("S", q1 * N.x + q2 * N.y + q3 * N.z)
P = S.locatenew("P", px * B.x)
A.set_vel(N, 0)
S.set_vel(N, S.pos_from(A).dt(N))
P.v2pt_theory(S, N, B)

Ixx = Ip / 2
Iyy = Ip / 2
Izz = Ip
I = inertia(B, Ixx, Iyy, Izz, 0, 0, 0)
rb = RigidBody("rb", S, B, m, (I, S))
rb.set_potential_energy(-m * g * (rb.masscenter.pos_from(A) & N.z) + k / 2 * (P.pos_from(A)).magnitude() ** 2)

L = Lagrangian(N, rb)
print("{} = {}\n".format("L", msprint(L)))

lm = LagrangesMethod(L, q)
print(msprint(lm.form_lagranges_equations()))