def test_n_link_pendulum_on_cart_higher_order():
    l0, m0 = symbols("l0 m0")
    l1, m1 = symbols("l1 m1")
    m2 = symbols("m2")
    g = symbols("g")
    q0, q1, q2 = dynamicsymbols("q0 q1 q2")
    u0, u1, u2 = dynamicsymbols("u0 u1 u2")
    F, T1 = dynamicsymbols("F T1")

    kane1 = models.n_link_pendulum_on_cart(2)
    massmatrix1 = Matrix([[m0 + m1 + m2, -l0*m1*cos(q1) - l0*m2*cos(q1),
                           -l1*m2*cos(q2)],
                          [-l0*m1*cos(q1) - l0*m2*cos(q1), l0**2*m1 + l0**2*m2,
                           l0*l1*m2*(sin(q1)*sin(q2) + cos(q1)*cos(q2))],
                          [-l1*m2*cos(q2),
                           l0*l1*m2*(sin(q1)*sin(q2) + cos(q1)*cos(q2)),
                           l1**2*m2]])
    forcing1 = Matrix([[-l0*m1*u1**2*sin(q1) - l0*m2*u1**2*sin(q1) -
                        l1*m2*u2**2*sin(q2) + F],
                       [g*l0*m1*sin(q1) + g*l0*m2*sin(q1) -
                        l0*l1*m2*(sin(q1)*cos(q2) - sin(q2)*cos(q1))*u2**2],
                       [g*l1*m2*sin(q2) - l0*l1*m2*(-sin(q1)*cos(q2) +
                                                    sin(q2)*cos(q1))*u1**2]])
    assert simplify(massmatrix1 - kane1.mass_matrix) == zeros(3)
    assert simplify(forcing1 - kane1.forcing) == Matrix([0, 0, 0])
예제 #2
1
def test_one_dof():
    # This is for a 1 dof spring-mass-damper case.
    # It is described in more detail in the KanesMethod docstring.
    q, u = dynamicsymbols('q u')
    qd, ud = dynamicsymbols('q u', 1)
    m, c, k = symbols('m c k')
    N = ReferenceFrame('N')
    P = Point('P')
    P.set_vel(N, u * N.x)

    kd = [qd - u]
    FL = [(P, (-k * q - c * u) * N.x)]
    pa = Particle('pa', P, m)
    BL = [pa]

    KM = KanesMethod(N, [q], [u], kd)
    # The old input format raises a deprecation warning, so catch it here so
    # it doesn't cause py.test to fail.
    with warnings.catch_warnings():
        warnings.filterwarnings("ignore", category=SymPyDeprecationWarning)
        KM.kanes_equations(FL, BL)

    MM = KM.mass_matrix
    forcing = KM.forcing
    rhs = MM.inv() * forcing
    assert expand(rhs[0]) == expand(-(q * k + u * c) / m)

    assert simplify(KM.rhs() -
                    KM.mass_matrix_full.LUsolve(KM.forcing_full)) == zeros(2, 1)

    assert (KM.linearize(A_and_B=True, )[0] == Matrix([[0, 1], [-k/m, -c/m]]))
def test_multi_mass_spring_damper_inputs():

    c0, k0, m0 = symbols("c0 k0 m0")
    g = symbols("g")
    v0, x0, f0 = dynamicsymbols("v0 x0 f0")

    kane1 = models.multi_mass_spring_damper(1)
    massmatrix1 = Matrix([[m0]])
    forcing1 = Matrix([[-c0*v0 - k0*x0]])
    assert simplify(massmatrix1 - kane1.mass_matrix) == Matrix([0])
    assert simplify(forcing1 - kane1.forcing) == Matrix([0])

    kane2 = models.multi_mass_spring_damper(1, True)
    massmatrix2 = Matrix([[m0]])
    forcing2 = Matrix([[-c0*v0 + g*m0 - k0*x0]])
    assert simplify(massmatrix2 - kane2.mass_matrix) == Matrix([0])
    assert simplify(forcing2 - kane2.forcing) == Matrix([0])

    kane3 = models.multi_mass_spring_damper(1, True, True)
    massmatrix3 = Matrix([[m0]])
    forcing3 = Matrix([[-c0*v0 + g*m0 - k0*x0 + f0]])
    assert simplify(massmatrix3 - kane3.mass_matrix) == Matrix([0])
    assert simplify(forcing3 - kane3.forcing) == Matrix([0])

    kane4 = models.multi_mass_spring_damper(1, False, True)
    massmatrix4 = Matrix([[m0]])
    forcing4 = Matrix([[-c0*v0 - k0*x0 + f0]])
    assert simplify(massmatrix4 - kane4.mass_matrix) == Matrix([0])
    assert simplify(forcing4 - kane4.forcing) == Matrix([0])
예제 #4
0
def test_input_format():
    # 1 dof problem from test_one_dof
    q, u = dynamicsymbols('q u')
    qd, ud = dynamicsymbols('q u', 1)
    m, c, k = symbols('m c k')
    N = ReferenceFrame('N')
    P = Point('P')
    P.set_vel(N, u * N.x)

    kd = [qd - u]
    FL = [(P, (-k * q - c * u) * N.x)]
    pa = Particle('pa', P, m)
    BL = [pa]

    KM = KanesMethod(N, [q], [u], kd)
    # test for input format kane.kanes_equations((body1, body2, particle1))
    assert KM.kanes_equations(BL)[0] == Matrix([0])
    # test for input format kane.kanes_equations(bodies=(body1, body 2), loads=(load1,load2))
    assert KM.kanes_equations(bodies=BL, loads=None)[0] == Matrix([0])
    # test for input format kane.kanes_equations(bodies=(body1, body 2), loads=None)
    assert KM.kanes_equations(BL, loads=None)[0] == Matrix([0])
    # test for input format kane.kanes_equations(bodies=(body1, body 2))
    assert KM.kanes_equations(BL)[0] == Matrix([0])
    # test for input format kane.kanes_equations(bodies=(body1, body2), loads=[])
    assert KM.kanes_equations(BL, [])[0] == Matrix([0])
    # test for error raised when a wrong force list (in this case a string) is provided
    raises(ValueError, lambda: KM._form_fr('bad input'))

    # 1 dof problem from test_one_dof with FL & BL in instance
    KM = KanesMethod(N, [q], [u], kd, bodies=BL, forcelist=FL)
    assert KM.kanes_equations()[0] == Matrix([-c*u - k*q])

    # 2 dof problem from test_two_dof
    q1, q2, u1, u2 = dynamicsymbols('q1 q2 u1 u2')
    q1d, q2d, u1d, u2d = dynamicsymbols('q1 q2 u1 u2', 1)
    m, c1, c2, k1, k2 = symbols('m c1 c2 k1 k2')
    N = ReferenceFrame('N')
    P1 = Point('P1')
    P2 = Point('P2')
    P1.set_vel(N, u1 * N.x)
    P2.set_vel(N, (u1 + u2) * N.x)
    kd = [q1d - u1, q2d - u2]

    FL = ((P1, (-k1 * q1 - c1 * u1 + k2 * q2 + c2 * u2) * N.x), (P2, (-k2 *
        q2 - c2 * u2) * N.x))
    pa1 = Particle('pa1', P1, m)
    pa2 = Particle('pa2', P2, m)
    BL = (pa1, pa2)

    KM = KanesMethod(N, q_ind=[q1, q2], u_ind=[u1, u2], kd_eqs=kd)
    # test for input format
    # kane.kanes_equations((body1, body2), (load1, load2))
    KM.kanes_equations(BL, FL)
    MM = KM.mass_matrix
    forcing = KM.forcing
    rhs = MM.inv() * forcing
    assert expand(rhs[0]) == expand((-k1 * q1 - c1 * u1 + k2 * q2 + c2 * u2)/m)
    assert expand(rhs[1]) == expand((k1 * q1 + c1 * u1 - 2 * k2 * q2 - 2 *
                                    c2 * u2) / m)
예제 #5
0
파일: test_kane.py 프로젝트: msgoff/sympy
def test_input_format():
    # 1 dof problem from test_one_dof
    q, u = dynamicsymbols("q u")
    qd, ud = dynamicsymbols("q u", 1)
    m, c, k = symbols("m c k")
    N = ReferenceFrame("N")
    P = Point("P")
    P.set_vel(N, u * N.x)

    kd = [qd - u]
    FL = [(P, (-k * q - c * u) * N.x)]
    pa = Particle("pa", P, m)
    BL = [pa]

    KM = KanesMethod(N, [q], [u], kd)
    # test for input format kane.kanes_equations((body1, body2, particle1))
    assert KM.kanes_equations(BL)[0] == Matrix([0])
    # test for input format kane.kanes_equations(bodies=(body1, body 2), loads=(load1,load2))
    assert KM.kanes_equations(bodies=BL, loads=None)[0] == Matrix([0])
    # test for input format kane.kanes_equations(bodies=(body1, body 2), loads=None)
    assert KM.kanes_equations(BL, loads=None)[0] == Matrix([0])
    # test for input format kane.kanes_equations(bodies=(body1, body 2))
    assert KM.kanes_equations(BL)[0] == Matrix([0])
    # test for error raised when a wrong force list (in this case a string) is provided
    from sympy.testing.pytest import raises

    raises(ValueError, lambda: KM._form_fr("bad input"))

    # 2 dof problem from test_two_dof
    q1, q2, u1, u2 = dynamicsymbols("q1 q2 u1 u2")
    q1d, q2d, u1d, u2d = dynamicsymbols("q1 q2 u1 u2", 1)
    m, c1, c2, k1, k2 = symbols("m c1 c2 k1 k2")
    N = ReferenceFrame("N")
    P1 = Point("P1")
    P2 = Point("P2")
    P1.set_vel(N, u1 * N.x)
    P2.set_vel(N, (u1 + u2) * N.x)
    kd = [q1d - u1, q2d - u2]

    FL = (
        (P1, (-k1 * q1 - c1 * u1 + k2 * q2 + c2 * u2) * N.x),
        (P2, (-k2 * q2 - c2 * u2) * N.x),
    )
    pa1 = Particle("pa1", P1, m)
    pa2 = Particle("pa2", P2, m)
    BL = (pa1, pa2)

    KM = KanesMethod(N, q_ind=[q1, q2], u_ind=[u1, u2], kd_eqs=kd)
    # test for input format
    # kane.kanes_equations((body1, body2), (load1, load2))
    KM.kanes_equations(BL, FL)
    MM = KM.mass_matrix
    forcing = KM.forcing
    rhs = MM.inv() * forcing
    assert expand(rhs[0]) == expand(
        (-k1 * q1 - c1 * u1 + k2 * q2 + c2 * u2) / m)
    assert expand(rhs[1]) == expand(
        (k1 * q1 + c1 * u1 - 2 * k2 * q2 - 2 * c2 * u2) / m)
예제 #6
0
파일: test_kane.py 프로젝트: msgoff/sympy
def test_parallel_axis():
    # This is for a 2 dof inverted pendulum on a cart.
    # This tests the parallel axis code in KanesMethod. The inertia of the
    # pendulum is defined about the hinge, not about the center of mass.

    # Defining the constants and knowns of the system
    gravity = symbols("g")
    k, ls = symbols("k ls")
    a, mA, mC = symbols("a mA mC")
    F = dynamicsymbols("F")
    Ix, Iy, Iz = symbols("Ix Iy Iz")

    # Declaring the Generalized coordinates and speeds
    q1, q2 = dynamicsymbols("q1 q2")
    q1d, q2d = dynamicsymbols("q1 q2", 1)
    u1, u2 = dynamicsymbols("u1 u2")
    u1d, u2d = dynamicsymbols("u1 u2", 1)

    # Creating reference frames
    N = ReferenceFrame("N")
    A = ReferenceFrame("A")

    A.orient(N, "Axis", [-q2, N.z])
    A.set_ang_vel(N, -u2 * N.z)

    # Origin of Newtonian reference frame
    O = Point("O")

    # Creating and Locating the positions of the cart, C, and the
    # center of mass of the pendulum, A
    C = O.locatenew("C", q1 * N.x)
    Ao = C.locatenew("Ao", a * A.y)

    # Defining velocities of the points
    O.set_vel(N, 0)
    C.set_vel(N, u1 * N.x)
    Ao.v2pt_theory(C, N, A)
    Cart = Particle("Cart", C, mC)
    Pendulum = RigidBody("Pendulum", Ao, A, mA, (inertia(A, Ix, Iy, Iz), C))

    # kinematical differential equations

    kindiffs = [q1d - u1, q2d - u2]

    bodyList = [Cart, Pendulum]

    forceList = [
        (Ao, -N.y * gravity * mA),
        (C, -N.y * gravity * mC),
        (C, -N.x * k * (q1 - ls)),
        (C, N.x * F),
    ]

    km = KanesMethod(N, [q1, q2], [u1, u2], kindiffs)
    with warns_deprecated_sympy():
        (fr, frstar) = km.kanes_equations(forceList, bodyList)
    mm = km.mass_matrix_full
    assert mm[3, 3] == Iz
예제 #7
0
파일: test_kane.py 프로젝트: alexako/sympy
def test_parallel_axis():
    # This is for a 2 dof inverted pendulum on a cart.
    # This tests the parallel axis code in KanesMethod. The inertia of the
    # pendulum is defined about the hinge, not about the center of mass.

    # Defining the constants and knowns of the system
    gravity = symbols('g')
    k, ls = symbols('k ls')
    a, mA, mC = symbols('a mA mC')
    F = dynamicsymbols('F')
    Ix, Iy, Iz = symbols('Ix Iy Iz')

    # Declaring the Generalized coordinates and speeds
    q1, q2 = dynamicsymbols('q1 q2')
    q1d, q2d = dynamicsymbols('q1 q2', 1)
    u1, u2 = dynamicsymbols('u1 u2')
    u1d, u2d = dynamicsymbols('u1 u2', 1)

    # Creating reference frames
    N = ReferenceFrame('N')
    A = ReferenceFrame('A')

    A.orient(N, 'Axis', [-q2, N.z])
    A.set_ang_vel(N, -u2 * N.z)

    # Origin of Newtonian reference frame
    O = Point('O')

    # Creating and Locating the positions of the cart, C, and the
    # center of mass of the pendulum, A
    C = O.locatenew('C', q1 * N.x)
    Ao = C.locatenew('Ao', a * A.y)

    # Defining velocities of the points
    O.set_vel(N, 0)
    C.set_vel(N, u1 * N.x)
    Ao.v2pt_theory(C, N, A)
    Cart = Particle('Cart', C, mC)
    Pendulum = RigidBody('Pendulum', Ao, A, mA, (inertia(A, Ix, Iy, Iz), C))

    # kinematical differential equations

    kindiffs = [q1d - u1, q2d - u2]

    bodyList = [Cart, Pendulum]

    forceList = [(Ao, -N.y * gravity * mA),
                 (C, -N.y * gravity * mC),
                 (C, -N.x * k * (q1 - ls)),
                 (C, N.x * F)]

    km = KanesMethod(N, [q1, q2], [u1, u2], kindiffs)
    with warnings.catch_warnings():
        warnings.filterwarnings("ignore", category=SymPyDeprecationWarning)
        (fr, frstar) = km.kanes_equations(forceList, bodyList)
    mm = km.mass_matrix_full
    assert mm[3, 3] == Iz
예제 #8
0
def test_parallel_axis():
    # This is for a 2 dof inverted pendulum on a cart.
    # This tests the parallel axis code in KanesMethod. The inertia of the
    # pendulum is defined about the hinge, not about the center of mass.

    # Defining the constants and knowns of the system
    gravity = symbols('g')
    k, ls = symbols('k ls')
    a, mA, mC = symbols('a mA mC')
    F = dynamicsymbols('F')
    Ix, Iy, Iz = symbols('Ix Iy Iz')

    # Declaring the Generalized coordinates and speeds
    q1, q2 = dynamicsymbols('q1 q2')
    q1d, q2d = dynamicsymbols('q1 q2', 1)
    u1, u2 = dynamicsymbols('u1 u2')
    u1d, u2d = dynamicsymbols('u1 u2', 1)

    # Creating reference frames
    N = ReferenceFrame('N')
    A = ReferenceFrame('A')

    A.orient(N, 'Axis', [-q2, N.z])
    A.set_ang_vel(N, -u2 * N.z)

    # Origin of Newtonian reference frame
    O = Point('O')

    # Creating and Locating the positions of the cart, C, and the
    # center of mass of the pendulum, A
    C = O.locatenew('C', q1 * N.x)
    Ao = C.locatenew('Ao', a * A.y)

    # Defining velocities of the points
    O.set_vel(N, 0)
    C.set_vel(N, u1 * N.x)
    Ao.v2pt_theory(C, N, A)
    Cart = Particle('Cart', C, mC)
    Pendulum = RigidBody('Pendulum', Ao, A, mA, (inertia(A, Ix, Iy, Iz), C))

    # kinematical differential equations

    kindiffs = [q1d - u1, q2d - u2]

    bodyList = [Cart, Pendulum]

    forceList = [(Ao, -N.y * gravity * mA), (C, -N.y * gravity * mC),
                 (C, -N.x * k * (q1 - ls)), (C, N.x * F)]

    km = KanesMethod(N, [q1, q2], [u1, u2], kindiffs)
    with warnings.catch_warnings():
        warnings.filterwarnings("ignore", category=SymPyDeprecationWarning)
        (fr, frstar) = km.kanes_equations(forceList, bodyList)
    mm = km.mass_matrix_full
    assert mm[3, 3] == Iz
예제 #9
0
def test_inertia():
    N = ReferenceFrame('N')
    ixx, iyy, izz = symbols('ixx iyy izz')
    ixy, iyz, izx = symbols('ixy iyz izx')
    assert inertia(N, ixx, iyy, izz) == (ixx * (N.x | N.x) + iyy *
            (N.y | N.y) + izz * (N.z | N.z))
    assert inertia(N, 0, 0, 0) == 0 * (N.x | N.x)
    assert inertia(N, ixx, iyy, izz, ixy, iyz, izx) == (ixx * (N.x | N.x) +
            ixy * (N.x | N.y) + izx * (N.x | N.z) + ixy * (N.y | N.x) + iyy *
        (N.y | N.y) + iyz * (N.y | N.z) + izx * (N.z | N.x) + iyz * (N.z |
            N.y) + izz * (N.z | N.z))
예제 #10
0
파일: test_kane.py 프로젝트: alexako/sympy
def test_input_format():
    # 1 dof problem from test_one_dof
    q, u = dynamicsymbols('q u')
    qd, ud = dynamicsymbols('q u', 1)
    m, c, k = symbols('m c k')
    N = ReferenceFrame('N')
    P = Point('P')
    P.set_vel(N, u * N.x)

    kd = [qd - u]
    FL = [(P, (-k * q - c * u) * N.x)]
    pa = Particle('pa', P, m)
    BL = [pa]

    KM = KanesMethod(N, [q], [u], kd)
    # test for input format kane.kanes_equations((body1, body2, particle1))
    assert KM.kanes_equations(BL)[0] == Matrix([0])
    # test for input format kane.kanes_equations(bodies=(body1, body 2), loads=(load1,load2))
    assert KM.kanes_equations(bodies=BL, loads=None)[0] == Matrix([0])
    # test for input format kane.kanes_equations(bodies=(body1, body 2), loads=None)
    assert KM.kanes_equations(BL, loads=None)[0] == Matrix([0])
    # test for input format kane.kanes_equations(bodies=(body1, body 2))
    assert KM.kanes_equations(BL)[0] == Matrix([0])
    # test for error raised when a wrong force list (in this case a string) is provided
    from sympy.utilities.pytest import raises
    raises(ValueError, lambda: KM._form_fr('bad input'))

    # 2 dof problem from test_two_dof
    q1, q2, u1, u2 = dynamicsymbols('q1 q2 u1 u2')
    q1d, q2d, u1d, u2d = dynamicsymbols('q1 q2 u1 u2', 1)
    m, c1, c2, k1, k2 = symbols('m c1 c2 k1 k2')
    N = ReferenceFrame('N')
    P1 = Point('P1')
    P2 = Point('P2')
    P1.set_vel(N, u1 * N.x)
    P2.set_vel(N, (u1 + u2) * N.x)
    kd = [q1d - u1, q2d - u2]

    FL = ((P1, (-k1 * q1 - c1 * u1 + k2 * q2 + c2 * u2) * N.x), (P2, (-k2 *
        q2 - c2 * u2) * N.x))
    pa1 = Particle('pa1', P1, m)
    pa2 = Particle('pa2', P2, m)
    BL = (pa1, pa2)

    KM = KanesMethod(N, q_ind=[q1, q2], u_ind=[u1, u2], kd_eqs=kd)
    # test for input format
    # kane.kanes_equations((body1, body2), (load1, load2))
    KM.kanes_equations(BL, FL)
    MM = KM.mass_matrix
    forcing = KM.forcing
    rhs = MM.inv() * forcing
    assert expand(rhs[0]) == expand((-k1 * q1 - c1 * u1 + k2 * q2 + c2 * u2)/m)
    assert expand(rhs[1]) == expand((k1 * q1 + c1 * u1 - 2 * k2 * q2 - 2 *
                                    c2 * u2) / m)
예제 #11
0
def test_inertia():
    N = ReferenceFrame("N")
    ixx, iyy, izz = symbols("ixx iyy izz")
    ixy, iyz, izx = symbols("ixy iyz izx")
    assert inertia(N, ixx, iyy, izz) == (ixx * (N.x | N.x) + iyy *
                                         (N.y | N.y) + izz * (N.z | N.z))
    assert inertia(N, 0, 0, 0) == 0 * (N.x | N.x)
    raises(TypeError, lambda: inertia(0, 0, 0, 0))
    assert inertia(
        N, ixx, iyy, izz, ixy, iyz,
        izx) == (ixx * (N.x | N.x) + ixy * (N.x | N.y) + izx * (N.x | N.z) +
                 ixy * (N.y | N.x) + iyy * (N.y | N.y) + iyz * (N.y | N.z) +
                 izx * (N.z | N.x) + iyz * (N.z | N.y) + izz * (N.z | N.z))
예제 #12
0
def test_default():
    body = Body('body')
    assert body.name == 'body'
    assert body.loads == []
    point = Point('body_masscenter')
    point.set_vel(body.frame, 0)
    com = body.masscenter
    frame = body.frame
    assert com.vel(frame) == point.vel(frame)
    assert body.mass == Symbol('body_mass')
    ixx, iyy, izz = symbols('body_ixx body_iyy body_izz')
    ixy, iyz, izx = symbols('body_ixy body_iyz body_izx')
    assert body.inertia == (inertia(body.frame, ixx, iyy, izz, ixy, iyz, izx),
                            body.masscenter)
예제 #13
0
def test_default():
    body = Body('body')
    assert body.name == 'body'
    assert body.loads == []
    point = Point('body_masscenter')
    point.set_vel(body.frame, 0)
    com = body.masscenter
    frame = body.frame
    assert com.vel(frame) == point.vel(frame)
    assert body.mass == Symbol('body_mass')
    ixx, iyy, izz = symbols('body_ixx body_iyy body_izz')
    ixy, iyz, izx = symbols('body_ixy body_iyz body_izx')
    assert body.inertia == (inertia(body.frame, ixx, iyy, izz, ixy, iyz,
                                    izx), body.masscenter)
예제 #14
0
def test_multi_mass_spring_damper_higher_order():
    c0, k0, m0 = symbols("c0 k0 m0")
    c1, k1, m1 = symbols("c1 k1 m1")
    c2, k2, m2 = symbols("c2 k2 m2")
    v0, x0 = dynamicsymbols("v0 x0")
    v1, x1 = dynamicsymbols("v1 x1")
    v2, x2 = dynamicsymbols("v2 x2")

    kane1 = models.multi_mass_spring_damper(3)
    massmatrix1 = Matrix([[m0 + m1 + m2, m1 + m2, m2], [m1 + m2, m1 + m2, m2],
                          [m2, m2, m2]])
    forcing1 = Matrix([[-c0 * v0 - k0 * x0], [-c1 * v1 - k1 * x1],
                       [-c2 * v2 - k2 * x2]])
    assert simplify(massmatrix1 - kane1.mass_matrix) == zeros(3)
    assert simplify(forcing1 - kane1.forcing) == Matrix([0, 0, 0])
예제 #15
0
파일: test_body.py 프로젝트: msgoff/sympy
def test_default():
    body = Body("body")
    assert body.name == "body"
    assert body.loads == []
    point = Point("body_masscenter")
    point.set_vel(body.frame, 0)
    com = body.masscenter
    frame = body.frame
    assert com.vel(frame) == point.vel(frame)
    assert body.mass == Symbol("body_mass")
    ixx, iyy, izz = symbols("body_ixx body_iyy body_izz")
    ixy, iyz, izx = symbols("body_ixy body_iyz body_izx")
    assert body.inertia == (
        inertia(body.frame, ixx, iyy, izz, ixy, iyz, izx),
        body.masscenter,
    )
예제 #16
0
def test_pend():
    q, u = dynamicsymbols('q u')
    qd, ud = dynamicsymbols('q u', 1)
    m, l, g = symbols('m l g')
    N = ReferenceFrame('N')
    P = Point('P')
    P.set_vel(N, -l * u * sin(q) * N.x + l * u * cos(q) * N.y)
    kd = [qd - u]

    FL = [(P, m * g * N.x)]
    pa = Particle('pa', P, m)
    BL = [pa]

    KM = KanesMethod(N, [q], [u], kd)
    with warnings.catch_warnings():
        warnings.filterwarnings("ignore", category=SymPyDeprecationWarning)
        KM.kanes_equations(FL, BL)
    MM = KM.mass_matrix
    forcing = KM.forcing
    rhs = MM.inv() * forcing
    rhs.simplify()
    assert expand(rhs[0]) == expand(-g / l * sin(q))
    assert simplify(KM.rhs() -
                    KM.mass_matrix_full.LUsolve(KM.forcing_full)) == zeros(
                        2, 1)
예제 #17
0
def test_apply_force():
    f, g = symbols('f g')
    q, x, v1, v2 = dynamicsymbols('q x v1 v2')
    P1 = Point('P1')
    P2 = Point('P2')
    B1 = Body('B1')
    B2 = Body('B2')
    N = ReferenceFrame('N')

    P1.set_vel(B1.frame, v1 * B1.x)
    P2.set_vel(B2.frame, v2 * B2.x)
    force = f * q * N.z  # time varying force

    B1.apply_force(force, P1, B2,
                   P2)  #applying equal and opposite force on moving points
    assert B1.loads == [(P1, force)]
    assert B2.loads == [(P2, -force)]

    g1 = B1.mass * g * N.y
    g2 = B2.mass * g * N.y

    B1.apply_force(g1)  #applying gravity on B1 masscenter
    B2.apply_force(g2)  #applying gravity on B2 masscenter

    assert B1.loads == [(P1, force), (B1.masscenter, g1)]
    assert B2.loads == [(P2, -force), (B2.masscenter, g2)]

    force2 = x * N.x

    B1.apply_force(
        force2, reaction_body=B2)  #Applying time varying force on masscenter

    assert B1.loads == [(P1, force), (B1.masscenter, force2 + g1)]
    assert B2.loads == [(P2, -force), (B2.masscenter, -force2 + g2)]
예제 #18
0
def dynamicsymbols(names, level=0):
    """Uses symbols and Function for functions of time.

    Creates a SymPy UndefinedFunction, which is then initialized as a function
    of a variable, the default being Symbol('t').

    Parameters
    ==========

    names : str
        Names of the dynamic symbols you want to create; works the same way as
        inputs to symbols
    level : int
        Level of differentiation of the returned function; d/dt once of t,
        twice of t, etc.

    Examples
    ========

    >>> from sympy.physics.vector import dynamicsymbols
    >>> from sympy import diff, Symbol
    >>> q1 = dynamicsymbols('q1')
    >>> q1
    q1(t)
    >>> diff(q1, Symbol('t'))
    Derivative(q1(t), t)

    """
    esses = symbols(names, cls=Function)
    t = dynamicsymbols._t
    if iterable(esses):
        esses = [reduce(diff, [t] * level, e(t)) for e in esses]
        return esses
    else:
        return reduce(diff, [t] * level, esses(t))
예제 #19
0
파일: test_kane.py 프로젝트: em3ndez/sympy
def test_one_dof():
    # This is for a 1 dof spring-mass-damper case.
    # It is described in more detail in the KanesMethod docstring.
    q, u = dynamicsymbols('q u')
    qd, ud = dynamicsymbols('q u', 1)
    m, c, k = symbols('m c k')
    N = ReferenceFrame('N')
    P = Point('P')
    P.set_vel(N, u * N.x)

    kd = [qd - u]
    FL = [(P, (-k * q - c * u) * N.x)]
    pa = Particle('pa', P, m)
    BL = [pa]

    KM = KanesMethod(N, [q], [u], kd)
    KM.kanes_equations(BL, FL)

    assert KM.bodies == BL

    MM = KM.mass_matrix
    forcing = KM.forcing
    rhs = MM.inv() * forcing
    assert expand(rhs[0]) == expand(-(q * k + u * c) / m)

    assert simplify(KM.rhs() -
                    KM.mass_matrix_full.LUsolve(KM.forcing_full)) == zeros(
                        2, 1)

    assert (KM.linearize(A_and_B=True, )[0] == Matrix([[0, 1],
                                                       [-k / m, -c / m]]))
예제 #20
0
def test_linearize_pendulum_lagrange_minimal():
    q1 = dynamicsymbols('q1')                     # angle of pendulum
    q1d = dynamicsymbols('q1', 1)                 # Angular velocity
    L, m, t = symbols('L, m, t')
    g = 9.8

    # Compose world frame
    N = ReferenceFrame('N')
    pN = Point('N*')
    pN.set_vel(N, 0)

    # A.x is along the pendulum
    A = N.orientnew('A', 'axis', [q1, N.z])
    A.set_ang_vel(N, q1d*N.z)

    # Locate point P relative to the origin N*
    P = pN.locatenew('P', L*A.x)
    P.v2pt_theory(pN, N, A)
    pP = Particle('pP', P, m)

    # Solve for eom with Lagranges method
    Lag = Lagrangian(N, pP)
    LM = LagrangesMethod(Lag, [q1], forcelist=[(P, m*g*N.x)], frame=N)
    LM.form_lagranges_equations()

    # Linearize
    A, B, inp_vec = LM.linearize([q1], [q1d], A_and_B=True)

    assert A == Matrix([[0, 1], [-9.8*cos(q1)/L, 0]])
    assert B == Matrix([])
예제 #21
0
def test_linearize_pendulum_lagrange_minimal():
    q1 = dynamicsymbols('q1')                     # angle of pendulum
    q1d = dynamicsymbols('q1', 1)                 # Angular velocity
    L, m, t = symbols('L, m, t')
    g = 9.8

    # Compose world frame
    N = ReferenceFrame('N')
    pN = Point('N*')
    pN.set_vel(N, 0)

    # A.x is along the pendulum
    A = N.orientnew('A', 'axis', [q1, N.z])
    A.set_ang_vel(N, q1d*N.z)

    # Locate point P relative to the origin N*
    P = pN.locatenew('P', L*A.x)
    P.v2pt_theory(pN, N, A)
    pP = Particle('pP', P, m)

    # Solve for eom with Lagranges method
    Lag = Lagrangian(N, pP)
    LM = LagrangesMethod(Lag, [q1], forcelist=[(P, m*g*N.x)], frame=N)
    LM.form_lagranges_equations()

    # Linearize
    A, B, inp_vec = LM.linearize([q1], [q1d], A_and_B=True)

    assert A == Matrix([[0, 1], [-9.8*cos(q1)/L, 0]])
    assert B == Matrix([])
예제 #22
0
def dynamicsymbols(names, level=0):
    """Uses symbols and Function for functions of time.

    Creates a SymPy UndefinedFunction, which is then initialized as a function
    of a variable, the default being Symbol('t').

    Parameters
    ==========

    names : str
        Names of the dynamic symbols you want to create; works the same way as
        inputs to symbols
    level : int
        Level of differentiation of the returned function; d/dt once of t,
        twice of t, etc.

    Examples
    ========

    >>> from sympy.physics.vector import dynamicsymbols
    >>> from sympy import diff, Symbol
    >>> q1 = dynamicsymbols('q1')
    >>> q1
    q1(t)
    >>> diff(q1, Symbol('t'))
    Derivative(q1(t), t)

    """
    esses = symbols(names, cls=Function)
    t = dynamicsymbols._t
    if iterable(esses):
        esses = [reduce(diff, [t] * level, e(t)) for e in esses]
        return esses
    else:
        return reduce(diff, [t] * level, esses(t))
예제 #23
0
def test_one_dof():
    # This is for a 1 dof spring-mass-damper case.
    # It is described in more detail in the KanesMethod docstring.
    q, u = dynamicsymbols('q u')
    qd, ud = dynamicsymbols('q u', 1)
    m, c, k = symbols('m c k')
    N = ReferenceFrame('N')
    P = Point('P')
    P.set_vel(N, u * N.x)

    kd = [qd - u]
    FL = [(P, (-k * q - c * u) * N.x)]
    pa = Particle('pa', P, m)
    BL = [pa]

    KM = KanesMethod(N, [q], [u], kd)
    # The old input format raises a deprecation warning, so catch it here so
    # it doesn't cause py.test to fail.
    with warns_deprecated_sympy():
        KM.kanes_equations(FL, BL)

    MM = KM.mass_matrix
    forcing = KM.forcing
    rhs = MM.inv() * forcing
    assert expand(rhs[0]) == expand(-(q * k + u * c) / m)

    assert simplify(KM.rhs() -
                    KM.mass_matrix_full.LUsolve(KM.forcing_full)) == zeros(
                        2, 1)

    assert (KM.linearize(A_and_B=True, )[0] == Matrix([[0, 1],
                                                       [-k / m, -c / m]]))
예제 #24
0
def test_linearize_pendulum_lagrange_nonminimal():
    q1, q2 = dynamicsymbols('q1:3')
    q1d, q2d = dynamicsymbols('q1:3', level=1)
    L, m, t = symbols('L, m, t')
    g = 9.8
    # Compose World Frame
    N = ReferenceFrame('N')
    pN = Point('N*')
    pN.set_vel(N, 0)
    # A.x is along the pendulum
    theta1 = atan(q2/q1)
    A = N.orientnew('A', 'axis', [theta1, N.z])
    # Create point P, the pendulum mass
    P = pN.locatenew('P1', q1*N.x + q2*N.y)
    P.set_vel(N, P.pos_from(pN).dt(N))
    pP = Particle('pP', P, m)
    # Constraint Equations
    f_c = Matrix([q1**2 + q2**2 - L**2])
    # Calculate the lagrangian, and form the equations of motion
    Lag = Lagrangian(N, pP)
    LM = LagrangesMethod(Lag, [q1, q2], hol_coneqs=f_c, forcelist=[(P, m*g*N.x)], frame=N)
    LM.form_lagranges_equations()
    # Compose operating point
    op_point = {q1: L, q2: 0, q1d: 0, q2d: 0, q1d.diff(t): 0, q2d.diff(t): 0}
    # Solve for multiplier operating point
    lam_op = LM.solve_multipliers(op_point=op_point)
    op_point.update(lam_op)
    # Perform the Linearization
    A, B, inp_vec = LM.linearize([q2], [q2d], [q1], [q1d],
            op_point=op_point, A_and_B=True)
    assert A == Matrix([[0, 1], [-9.8/L, 0]])
    assert B == Matrix([])
예제 #25
0
def test_linearize_pendulum_lagrange_nonminimal():
    q1, q2 = dynamicsymbols('q1:3')
    q1d, q2d = dynamicsymbols('q1:3', level=1)
    L, m, t = symbols('L, m, t')
    g = 9.8
    # Compose World Frame
    N = ReferenceFrame('N')
    pN = Point('N*')
    pN.set_vel(N, 0)
    # A.x is along the pendulum
    theta1 = atan(q2/q1)
    A = N.orientnew('A', 'axis', [theta1, N.z])
    # Create point P, the pendulum mass
    P = pN.locatenew('P1', q1*N.x + q2*N.y)
    P.set_vel(N, P.pos_from(pN).dt(N))
    pP = Particle('pP', P, m)
    # Constraint Equations
    f_c = Matrix([q1**2 + q2**2 - L**2])
    # Calculate the lagrangian, and form the equations of motion
    Lag = Lagrangian(N, pP)
    LM = LagrangesMethod(Lag, [q1, q2], hol_coneqs=f_c, forcelist=[(P, m*g*N.x)], frame=N)
    LM.form_lagranges_equations()
    # Compose operating point
    op_point = {q1: L, q2: 0, q1d: 0, q2d: 0, q1d.diff(t): 0, q2d.diff(t): 0}
    # Solve for multiplier operating point
    lam_op = LM.solve_multipliers(op_point=op_point)
    op_point.update(lam_op)
    # Perform the Linearization
    A, B, inp_vec = LM.linearize([q2], [q2d], [q1], [q1d],
            op_point=op_point, A_and_B=True)
    assert A == Matrix([[0, 1], [-9.8/L, 0]])
    assert B == Matrix([])
예제 #26
0
def test_apply_loads_on_multi_degree_freedom_holonomic_system():
    """Example based on: https://pydy.readthedocs.io/en/latest/examples/multidof-holonomic.html"""
    W = Body('W')  #Wall
    B = Body('B')  #Block
    P = Body('P')  #Pendulum
    b = Body('b')  #bob
    q1, q2 = dynamicsymbols('q1 q2')  #generalized coordinates
    k, c, g, kT = symbols('k c g kT')  #constants
    F, T = dynamicsymbols('F T')  #Specified forces

    #Applying forces
    B.apply_force(F * W.x)
    W.apply_force(k * q1 * W.x, reaction_body=B)  #Spring force
    W.apply_force(c * q1.diff() * W.x, reaction_body=B)  #dampner
    P.apply_force(P.mass * g * W.y)
    b.apply_force(b.mass * g * W.y)

    #Applying torques
    P.apply_torque(kT * q2 * W.z, reaction_body=b)
    P.apply_torque(T * W.z)

    assert B.loads == [(B.masscenter, (F - k * q1 - c * q1.diff()) * W.x)]
    assert P.loads == [(P.masscenter, P.mass * g * W.y),
                       (P.frame, (T + kT * q2) * W.z)]
    assert b.loads == [(b.masscenter, b.mass * g * W.y),
                       (b.frame, -kT * q2 * W.z)]
    assert W.loads == [(W.masscenter, (c * q1.diff() + k * q1) * W.x)]
예제 #27
0
def test_angular_momentum_and_linear_momentum():
    """A rod with length 2l, centroidal inertia I, and mass M along with a
    particle of mass m fixed to the end of the rod rotate with an angular rate
    of omega about point O which is fixed to the non-particle end of the rod.
    The rod's reference frame is A and the inertial frame is N."""
    m, M, l, I = symbols('m, M, l, I')
    omega = dynamicsymbols('omega')
    N = ReferenceFrame('N')
    a = ReferenceFrame('a')
    O = Point('O')
    Ac = O.locatenew('Ac', l * N.x)
    P = Ac.locatenew('P', l * N.x)
    O.set_vel(N, 0 * N.x)
    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)
    A = RigidBody('A', Ac, a, M, (I * outer(N.z, N.z), Ac))
    expected = 2 * m * omega * l * N.y + M * l * omega * N.y
    assert linear_momentum(N, A, Pa) == expected
    raises(TypeError, lambda: angular_momentum(N, N, A, Pa))
    raises(TypeError, lambda: angular_momentum(O, O, A, Pa))
    raises(TypeError, lambda: angular_momentum(O, N, O, Pa))
    expected = (I + M * l**2 + 4 * m * l**2) * omega * N.z
    assert angular_momentum(O, N, A, Pa) == expected
예제 #28
0
def test_inertia_of_point_mass():
    r, s, t, m = symbols('r s t m')
    N = ReferenceFrame('N')

    px = r * N.x
    I = inertia_of_point_mass(m, px, N)
    assert I == m * r**2 * (N.y | N.y) + m * r**2 * (N.z | N.z)

    py = s * N.y
    I = inertia_of_point_mass(m, py, N)
    assert I == m * s**2 * (N.x | N.x) + m * s**2 * (N.z | N.z)

    pz = t * N.z
    I = inertia_of_point_mass(m, pz, N)
    assert I == m * t**2 * (N.x | N.x) + m * t**2 * (N.y | N.y)

    p = px + py + pz
    I = inertia_of_point_mass(m, p, N)
    assert I == (m * (s**2 + t**2) * (N.x | N.x) -
                 m * r * s * (N.x | N.y) -
                 m * r * t * (N.x | N.z) -
                 m * r * s * (N.y | N.x) +
                 m * (r**2 + t**2) * (N.y | N.y) -
                 m * s * t * (N.y | N.z) -
                 m * r * t * (N.z | N.x) -
                 m * s * t * (N.z | N.y) +
                 m * (r**2 + s**2) * (N.z | N.z))
예제 #29
0
def test_aux():
    # Same as above, except we have 2 auxiliary speeds for the ground contact
    # point, which is known to be zero. In one case, we go through then
    # substitute the aux. speeds in at the end (they are zero, as well as their
    # derivative), in the other case, we use the built-in auxiliary speed part
    # of KanesMethod. The equations from each should be the same.
    q1, q2, q3, u1, u2, u3 = dynamicsymbols('q1 q2 q3 u1 u2 u3')
    q1d, q2d, q3d, u1d, u2d, u3d = dynamicsymbols('q1 q2 q3 u1 u2 u3', 1)
    u4, u5, f1, f2 = dynamicsymbols('u4, u5, f1, f2')
    u4d, u5d = dynamicsymbols('u4, u5', 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])
    w_R_N_qd = R.ang_vel_in(N)
    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)
    Dmc.a2pt_theory(C, N, R)

    I = inertia(L, m / 4 * r**2, m / 2 * r**2, m / 4 * r**2)

    kd = [dot(R.ang_vel_in(N) - w_R_N_qd, uv) for uv in L]

    ForceList = [(Dmc, -m * g * Y.z), (C, f1 * L.x + f2 * (Y.z ^ L.x))]
    BodyD = RigidBody('BodyD', Dmc, R, m, (I, Dmc))
    BodyList = [BodyD]

    KM = KanesMethod(N,
                     q_ind=[q1, q2, q3],
                     u_ind=[u1, u2, u3, u4, u5],
                     kd_eqs=kd)
    with warnings.catch_warnings():
        warnings.filterwarnings("ignore", category=SymPyDeprecationWarning)
        (fr, frstar) = KM.kanes_equations(ForceList, BodyList)
    fr = fr.subs({u4d: 0, u5d: 0}).subs({u4: 0, u5: 0})
    frstar = frstar.subs({u4d: 0, u5d: 0}).subs({u4: 0, u5: 0})

    KM2 = KanesMethod(N,
                      q_ind=[q1, q2, q3],
                      u_ind=[u1, u2, u3],
                      kd_eqs=kd,
                      u_auxiliary=[u4, u5])
    with warnings.catch_warnings():
        warnings.filterwarnings("ignore", category=SymPyDeprecationWarning)
        (fr2, frstar2) = KM2.kanes_equations(ForceList, BodyList)
    fr2 = fr2.subs({u4d: 0, u5d: 0}).subs({u4: 0, u5: 0})
    frstar2 = frstar2.subs({u4d: 0, u5d: 0}).subs({u4: 0, u5: 0})

    frstar.simplify()
    frstar2.simplify()

    assert (fr - fr2).expand() == Matrix([0, 0, 0, 0, 0])
    assert (frstar - frstar2).expand() == Matrix([0, 0, 0, 0, 0])
예제 #30
0
def test_multi_mass_spring_damper_higher_order():
    c0, k0, m0 = symbols("c0 k0 m0")
    c1, k1, m1 = symbols("c1 k1 m1")
    c2, k2, m2 = symbols("c2 k2 m2")
    v0, x0 = dynamicsymbols("v0 x0")
    v1, x1 = dynamicsymbols("v1 x1")
    v2, x2 = dynamicsymbols("v2 x2")

    kane1 = models.multi_mass_spring_damper(3)
    massmatrix1 = Matrix([[m0 + m1 + m2, m1 + m2, m2],
                          [m1 + m2, m1 + m2, m2],
                          [m2, m2, m2]])
    forcing1 = Matrix([[-c0*v0 - k0*x0],
                       [-c1*v1 - k1*x1],
                       [-c2*v2 - k2*x2]])
    assert simplify(massmatrix1 - kane1.mass_matrix) == zeros(3)
    assert simplify(forcing1 - kane1.forcing) == Matrix([0, 0, 0])
예제 #31
0
def test_clear_load():
    a = symbols('a')
    P = Point('P')
    B = Body('B')
    force = a * B.z
    B.apply_force(force, P)
    assert B.loads == [(P, force)]
    B.clear_loads()
    assert B.loads == []
예제 #32
0
def dynamicsymbols(names, level=0, **assumptions):
    """Uses symbols and Function for functions of time.

    Creates a SymPy UndefinedFunction, which is then initialized as a function
    of a variable, the default being Symbol('t').

    Parameters
    ==========

    names : str
        Names of the dynamic symbols you want to create; works the same way as
        inputs to symbols
    level : int
        Level of differentiation of the returned function; d/dt once of t,
        twice of t, etc.
    assumptions :
        - real(bool) : This is used to set the dynamicsymbol as real,
                    by default is False.
        - positive(bool) : This is used to set the dynamicsymbol as positive,
                    by default is False.
        - commutative(bool) : This is used to set the commutative property of
                    a dynamicsymbol, by default is True.
        - integer(bool) : This is used to set the dynamicsymbol as integer,
                    by default is False.

    Examples
    ========

    >>> from sympy.physics.vector import dynamicsymbols
    >>> from sympy import diff, Symbol
    >>> q1 = dynamicsymbols('q1')
    >>> q1
    q1(t)
    >>> q2 = dynamicsymbols('q2', real=True)
    >>> q2.is_real
    True
    >>> q3 = dynamicsymbols('q3', positive=True)
    >>> q3.is_positive
    True
    >>> q4, q5 = dynamicsymbols('q4,q5', commutative=False)
    >>> bool(q4*q5 != q5*q4)
    True
    >>> q6 = dynamicsymbols('q6', integer=True)
    >>> q6.is_integer
    True
    >>> diff(q1, Symbol('t'))
    Derivative(q1(t), t)

    """
    esses = symbols(names, cls=Function, **assumptions)
    t = dynamicsymbols._t
    if iterable(esses):
        esses = [reduce(diff, [t] * level, e(t)) for e in esses]
        return esses
    else:
        return reduce(diff, [t] * level, esses(t))
예제 #33
0
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
예제 #34
0
def test_center_of_mass():
    a = ReferenceFrame('a')
    m = symbols('m', real=True)
    p1 = Particle('p1', Point('p1_pt'), S.One)
    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
예제 #35
0
def test_apply_force_multiple_one_point():
    a, b = symbols('a b')
    P = Point('P')
    B = Body('B')
    f1 = a * B.x
    f2 = b * B.y
    B.apply_force(f1, P)
    assert B.loads == [(P, f1)]
    B.apply_force(f2, P)
    assert B.loads == [(P, f1 + f2)]
예제 #36
0
파일: test_kane.py 프로젝트: alexako/sympy
def test_aux():
    # Same as above, except we have 2 auxiliary speeds for the ground contact
    # point, which is known to be zero. In one case, we go through then
    # substitute the aux. speeds in at the end (they are zero, as well as their
    # derivative), in the other case, we use the built-in auxiliary speed part
    # of KanesMethod. The equations from each should be the same.
    q1, q2, q3, u1, u2, u3 = dynamicsymbols('q1 q2 q3 u1 u2 u3')
    q1d, q2d, q3d, u1d, u2d, u3d = dynamicsymbols('q1 q2 q3 u1 u2 u3', 1)
    u4, u5, f1, f2 = dynamicsymbols('u4, u5, f1, f2')
    u4d, u5d = dynamicsymbols('u4, u5', 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])
    w_R_N_qd = R.ang_vel_in(N)
    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)
    Dmc.a2pt_theory(C, N, R)

    I = inertia(L, m / 4 * r**2, m / 2 * r**2, m / 4 * r**2)

    kd = [dot(R.ang_vel_in(N) - w_R_N_qd, uv) for uv in L]

    ForceList = [(Dmc, - m * g * Y.z), (C, f1 * L.x + f2 * (Y.z ^ L.x))]
    BodyD = RigidBody('BodyD', Dmc, R, m, (I, Dmc))
    BodyList = [BodyD]

    KM = KanesMethod(N, q_ind=[q1, q2, q3], u_ind=[u1, u2, u3, u4, u5],
                     kd_eqs=kd)
    with warnings.catch_warnings():
        warnings.filterwarnings("ignore", category=SymPyDeprecationWarning)
        (fr, frstar) = KM.kanes_equations(ForceList, BodyList)
    fr = fr.subs({u4d: 0, u5d: 0}).subs({u4: 0, u5: 0})
    frstar = frstar.subs({u4d: 0, u5d: 0}).subs({u4: 0, u5: 0})

    KM2 = KanesMethod(N, q_ind=[q1, q2, q3], u_ind=[u1, u2, u3], kd_eqs=kd,
                      u_auxiliary=[u4, u5])
    with warnings.catch_warnings():
        warnings.filterwarnings("ignore", category=SymPyDeprecationWarning)
        (fr2, frstar2) = KM2.kanes_equations(ForceList, BodyList)
    fr2 = fr2.subs({u4d: 0, u5d: 0}).subs({u4: 0, u5: 0})
    frstar2 = frstar2.subs({u4d: 0, u5d: 0}).subs({u4: 0, u5: 0})

    frstar.simplify()
    frstar2.simplify()

    assert (fr - fr2).expand() == Matrix([0, 0, 0, 0, 0])
    assert (frstar - frstar2).expand() == Matrix([0, 0, 0, 0, 0])
예제 #37
0
def test_center_of_mass():
    a = ReferenceFrame("a")
    m = symbols("m", real=True)
    p1 = Particle("p1", Point("p1_pt"), S.One)
    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
예제 #38
0
def test_find_dynamicsymbols():
    a, b = symbols('a, b')
    x, y, z = dynamicsymbols('x, y, z')
    expr = Matrix([[a*x + b, x*y.diff() + y],
                   [x.diff().diff(), z + sin(z.diff())]])
    # Test finding all dynamicsymbols
    sol = {x, y.diff(), y, x.diff().diff(), z, z.diff()}
    assert find_dynamicsymbols(expr) == sol
    # Test finding all but those in sym_list
    exclude = [x, y, z]
    sol = {y.diff(), x.diff().diff(), z.diff()}
    assert find_dynamicsymbols(expr, exclude) == sol
예제 #39
0
def test_find_dynamicsymbols():
    a, b = symbols('a, b')
    x, y, z = dynamicsymbols('x, y, z')
    expr = Matrix([[a * x + b, x * y.diff() + y],
                   [x.diff().diff(), z + sin(z.diff())]])
    # Test finding all dynamicsymbols
    sol = {x, y.diff(), y, x.diff().diff(), z, z.diff()}
    assert find_dynamicsymbols(expr) == sol
    # Test finding all but those in sym_list
    exclude = [x, y, z]
    sol = {y.diff(), x.diff().diff(), z.diff()}
    assert find_dynamicsymbols(expr, exclude) == sol
예제 #40
0
def test_apply_torque():
    t = symbols('t')
    q = dynamicsymbols('q')
    B1 = Body('B1')
    B2 = Body('B2')
    N = ReferenceFrame('N')
    torque = t * q * N.x

    B1.apply_torque(torque, B2)  #Applying equal and opposite torque
    assert B1.loads == [(B1.frame, torque)]
    assert B2.loads == [(B2.frame, -torque)]

    torque2 = t * N.y
    B1.apply_torque(torque2)
    assert B1.loads == [(B1.frame, torque + torque2)]
예제 #41
0
def test_gravity():
    N = ReferenceFrame('N')
    m, M, g = symbols('m M g')
    F1, F2 = dynamicsymbols('F1 F2')
    po = Point('po')
    pa = Particle('pa', po, m)
    A = ReferenceFrame('A')
    P = Point('P')
    I = outer(A.x, A.x)
    B = RigidBody('B', P, A, M, (I, P))
    forceList = [(po, F1), (P, F2)]
    forceList.extend(gravity(g*N.y, pa, B))
    l = [(po, F1), (P, F2), (po, g*m*N.y), (P, g*M*N.y)]

    for i in range(len(l)):
        for j in range(len(l[i])):
            assert forceList[i][j] == l[i][j]
예제 #42
0
def test_kinetic_energy():
    m, M, l1 = symbols('m M l1')
    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))
    assert 0 == (kinetic_energy(N, Pa, A) - (M*l1**2*omega**2/2
            + 2*l1**2*m*omega**2 + omega**2/2)).expand()
예제 #43
0
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.potential_energy = m * g * h
    A.potential_energy = M * g * H
    assert potential_energy(A, Pa) == m * g * h + M * g * H
예제 #44
0
def test_find_dynamicsymbols():
    a, b = symbols('a, b')
    x, y, z = dynamicsymbols('x, y, z')
    expr = Matrix([[a*x + b, x*y.diff() + y],
                   [x.diff().diff(), z + sin(z.diff())]])
    # Test finding all dynamicsymbols
    sol = {x, y.diff(), y, x.diff().diff(), z, z.diff()}
    assert find_dynamicsymbols(expr) == sol
    # Test finding all but those in sym_list
    exclude_list = [x, y, z]
    sol = {y.diff(), x.diff().diff(), z.diff()}
    assert find_dynamicsymbols(expr, exclude=exclude_list) == sol
    # Test finding all dynamicsymbols in a vector with a given reference frame
    d, e, f = dynamicsymbols('d, e, f')
    A = ReferenceFrame('A')
    v = d * A.x + e * A.y + f * A.z
    sol = {d, e, f}
    assert find_dynamicsymbols(v, reference_frame=A) == sol
    # Test if a ValueError is raised on supplying only a vector as input
    raises(ValueError, lambda: find_dynamicsymbols(v))
예제 #45
0
파일: test_kane.py 프로젝트: alexako/sympy
def test_two_dof():
    # This is for a 2 d.o.f., 2 particle spring-mass-damper.
    # The first coordinate is the displacement of the first particle, and the
    # second is the relative displacement between the first and second
    # particles. Speeds are defined as the time derivatives of the particles.
    q1, q2, u1, u2 = dynamicsymbols('q1 q2 u1 u2')
    q1d, q2d, u1d, u2d = dynamicsymbols('q1 q2 u1 u2', 1)
    m, c1, c2, k1, k2 = symbols('m c1 c2 k1 k2')
    N = ReferenceFrame('N')
    P1 = Point('P1')
    P2 = Point('P2')
    P1.set_vel(N, u1 * N.x)
    P2.set_vel(N, (u1 + u2) * N.x)
    kd = [q1d - u1, q2d - u2]

    # Now we create the list of forces, then assign properties to each
    # particle, then create a list of all particles.
    FL = [(P1, (-k1 * q1 - c1 * u1 + k2 * q2 + c2 * u2) * N.x), (P2, (-k2 *
        q2 - c2 * u2) * N.x)]
    pa1 = Particle('pa1', P1, m)
    pa2 = Particle('pa2', P2, m)
    BL = [pa1, pa2]

    # Finally we create the KanesMethod object, specify the inertial frame,
    # pass relevant information, and form Fr & Fr*. Then we calculate the mass
    # matrix and forcing terms, and finally solve for the udots.
    KM = KanesMethod(N, q_ind=[q1, q2], u_ind=[u1, u2], kd_eqs=kd)
    # The old input format raises a deprecation warning, so catch it here so
    # it doesn't cause py.test to fail.
    with warnings.catch_warnings():
        warnings.filterwarnings("ignore", category=SymPyDeprecationWarning)
        KM.kanes_equations(FL, BL)
    MM = KM.mass_matrix
    forcing = KM.forcing
    rhs = MM.inv() * forcing
    assert expand(rhs[0]) == expand((-k1 * q1 - c1 * u1 + k2 * q2 + c2 * u2)/m)
    assert expand(rhs[1]) == expand((k1 * q1 + c1 * u1 - 2 * k2 * q2 - 2 *
                                    c2 * u2) / m)

    assert simplify(KM.rhs() -
                    KM.mass_matrix_full.LUsolve(KM.forcing_full)) == zeros(4, 1)
예제 #46
0
def test_msubs():
    a, b = symbols('a, b')
    x, y, z = dynamicsymbols('x, y, z')
    # Test simple substitution
    expr = Matrix([[a*x + b, x*y.diff() + y],
                   [x.diff().diff(), z + sin(z.diff())]])
    sol = Matrix([[a + b, y],
                  [x.diff().diff(), 1]])
    sd = {x: 1, z: 1, z.diff(): 0, y.diff(): 0}
    assert msubs(expr, sd) == sol
    # Test smart substitution
    expr = cos(x + y)*tan(x + y) + b*x.diff()
    sd = {x: 0, y: pi/2, x.diff(): 1}
    assert msubs(expr, sd, smart=True) == b + 1
    N = ReferenceFrame('N')
    v = x*N.x + y*N.y
    d = x*(N.x|N.x) + y*(N.y|N.y)
    v_sol = 1*N.y
    d_sol = 1*(N.y|N.y)
    sd = {x: 0, y: 1}
    assert msubs(v, sd) == v_sol
    assert msubs(d, sd) == d_sol
예제 #47
0
def test_angular_momentum_and_linear_momentum():
    """A rod with length 2l, centroidal inertia I, and mass M along with a
    particle of mass m fixed to the end of the rod rotate with an angular rate
    of omega about point O which is fixed to the non-particle end of the rod.
    The rod's reference frame is A and the inertial frame is N."""
    m, M, l, I = symbols('m, M, l, I')
    omega = dynamicsymbols('omega')
    N = ReferenceFrame('N')
    a = ReferenceFrame('a')
    O = Point('O')
    Ac = O.locatenew('Ac', l * N.x)
    P = Ac.locatenew('P', l * N.x)
    O.set_vel(N, 0 * N.x)
    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)
    A = RigidBody('A', Ac, a, M, (I * outer(N.z, N.z), Ac))
    expected = 2 * m * omega * l * N.y + M * l * omega * N.y
    assert linear_momentum(N, A, Pa) == expected
    expected = (I + M * l**2 + 4 * m * l**2) * omega * N.z
    assert angular_momentum(O, N, A, Pa) == expected
예제 #48
0
def test_lagrange_2forces():
    ### Equations for two damped springs in serie with two forces

    ### generalized coordinates
    qs = q1, q2 = dynamicsymbols('q1, q2')
    ### generalized speeds
    qds = q1d, q2d = dynamicsymbols('q1, q2', 1)

    ### Mass, spring strength, friction coefficient
    m, k, nu = symbols('m, k, nu')

    N = ReferenceFrame('N')
    O = Point('O')

    ### Two points
    P1 = O.locatenew('P1', q1 * N.x)
    P1.set_vel(N, q1d * N.x)
    P2 = O.locatenew('P1', q2 * N.x)
    P2.set_vel(N, q2d * N.x)

    pP1 = Particle('pP1', P1, m)
    pP1.potential_energy = k * q1**2 / 2

    pP2 = Particle('pP2', P2, m)
    pP2.potential_energy = k * (q1 - q2)**2 / 2

    #### Friction forces
    forcelist = [(P1, - nu * q1d * N.x),
                 (P2, - nu * q2d * N.x)]
    lag = Lagrangian(N, pP1, pP2)

    l_method = LagrangesMethod(lag, (q1, q2), forcelist=forcelist, frame=N)
    l_method.form_lagranges_equations()

    eq1 = l_method.eom[0]
    assert eq1.diff(q1d) == nu
    eq2 = l_method.eom[1]
    assert eq2.diff(q2d) == nu
예제 #49
0
def test_linearize_pendulum_kane_minimal():
    q1 = dynamicsymbols('q1')                     # angle of pendulum
    u1 = dynamicsymbols('u1')                     # Angular velocity
    q1d = dynamicsymbols('q1', 1)                 # Angular velocity
    L, m, t = symbols('L, m, t')
    g = 9.8

    # Compose world frame
    N = ReferenceFrame('N')
    pN = Point('N*')
    pN.set_vel(N, 0)

    # A.x is along the pendulum
    A = N.orientnew('A', 'axis', [q1, N.z])
    A.set_ang_vel(N, u1*N.z)

    # Locate point P relative to the origin N*
    P = pN.locatenew('P', L*A.x)
    P.v2pt_theory(pN, N, A)
    pP = Particle('pP', P, m)

    # Create Kinematic Differential Equations
    kde = Matrix([q1d - u1])

    # Input the force resultant at P
    R = m*g*N.x

    # Solve for eom with kanes method
    KM = KanesMethod(N, q_ind=[q1], u_ind=[u1], kd_eqs=kde)
    with warnings.catch_warnings():
        warnings.filterwarnings("ignore", category=SymPyDeprecationWarning)
        (fr, frstar) = KM.kanes_equations([(P, R)], [pP])

    # Linearize
    A, B, inp_vec = KM.linearize(A_and_B=True, new_method=True, simplify=True)

    assert A == Matrix([[0, 1], [-9.8*cos(q1)/L, 0]])
    assert B == Matrix([])
예제 #50
0
파일: test_kane.py 프로젝트: asmeurer/sympy
def test_pend():
    q, u = dynamicsymbols('q u')
    qd, ud = dynamicsymbols('q u', 1)
    m, l, g = symbols('m l g')
    N = ReferenceFrame('N')
    P = Point('P')
    P.set_vel(N, -l * u * sin(q) * N.x + l * u * cos(q) * N.y)
    kd = [qd - u]

    FL = [(P, m * g * N.x)]
    pa = Particle('pa', P, m)
    BL = [pa]

    KM = KanesMethod(N, [q], [u], kd)
    with warns_deprecated_sympy():
        KM.kanes_equations(FL, BL)
    MM = KM.mass_matrix
    forcing = KM.forcing
    rhs = MM.inv() * forcing
    rhs.simplify()
    assert expand(rhs[0]) == expand(-g / l * sin(q))
    assert simplify(KM.rhs() -
                    KM.mass_matrix_full.LUsolve(KM.forcing_full)) == zeros(2, 1)
예제 #51
0
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.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
예제 #52
0
from sympy.core.backend import symbols, Matrix, atan, zeros
from sympy import simplify
from sympy.physics.mechanics import (dynamicsymbols, Particle, Point,
                                     ReferenceFrame, SymbolicSystem)
from sympy.utilities.pytest import raises

# This class is going to be tested using a simple pendulum set up in x and y
# coordinates
x, y, u, v, lam = dynamicsymbols('x y u v lambda')
m, l, g = symbols('m l g')

# Set up the different forms the equations can take
#       [1] Explicit form where the kinematics and dynamics are combined
#           x' = F(x, t, r, p)
#
#       [2] Implicit form where the kinematics and dynamics are combined
#           M(x, p) x' = F(x, t, r, p)
#
#       [3] Implicit form where the kinematics and dynamics are separate
#           M(q, p) u' = F(q, u, t, r, p)
#           q' = G(q, u, t, r, p)
dyn_implicit_mat = Matrix([[1, 0, -x/m],
                           [0, 1, -y/m],
                           [0, 0, l**2/m]])

dyn_implicit_rhs = Matrix([0, 0, u**2 + v**2 - g*y])

comb_implicit_mat = Matrix([[1, 0, 0, 0, 0],
                            [0, 1, 0, 0, 0],
                            [0, 0, 1, 0, -x/m],
                            [0, 0, 0, 1, -y/m],
예제 #53
0
    def orient(self, parent, rot_type, amounts, rot_order=''):
        """Defines the orientation of this frame relative to a parent frame.

        Parameters
        ==========

        parent : ReferenceFrame
            The frame that this ReferenceFrame will have its orientation matrix
            defined in relation to.
        rot_type : str
            The type of orientation matrix that is being created. Supported
            types are 'Body', 'Space', 'Quaternion', 'Axis', and 'DCM'.
            See examples for correct usage.
        amounts : list OR value
            The quantities that the orientation matrix will be defined by.
            In case of rot_type='DCM', value must be a
            sympy.matrices.MatrixBase object (or subclasses of it).
        rot_order : str
            If applicable, the order of a series of rotations.

        Examples
        ========

        >>> from sympy.physics.vector import ReferenceFrame, Vector
        >>> from sympy import symbols, eye, ImmutableMatrix
        >>> q0, q1, q2, q3 = symbols('q0 q1 q2 q3')
        >>> N = ReferenceFrame('N')
        >>> B = ReferenceFrame('B')

        Now we have a choice of how to implement the orientation. First is
        Body. Body orientation takes this reference frame through three
        successive simple rotations. Acceptable rotation orders are of length
        3, expressed in XYZ or 123, and cannot have a rotation about about an
        axis twice in a row.

        >>> B.orient(N, 'Body', [q1, q2, q3], '123')
        >>> B.orient(N, 'Body', [q1, q2, 0], 'ZXZ')
        >>> B.orient(N, 'Body', [0, 0, 0], 'XYX')

        Next is Space. Space is like Body, but the rotations are applied in the
        opposite order.

        >>> B.orient(N, 'Space', [q1, q2, q3], '312')

        Next is Quaternion. This orients the new ReferenceFrame with
        Quaternions, defined as a finite rotation about lambda, a unit vector,
        by some amount theta.
        This orientation is described by four parameters:
        q0 = cos(theta/2)
        q1 = lambda_x sin(theta/2)
        q2 = lambda_y sin(theta/2)
        q3 = lambda_z sin(theta/2)
        Quaternion does not take in a rotation order.

        >>> B.orient(N, 'Quaternion', [q0, q1, q2, q3])

        Next is Axis. This is a rotation about an arbitrary, non-time-varying
        axis by some angle. The axis is supplied as a Vector. This is how
        simple rotations are defined.

        >>> B.orient(N, 'Axis', [q1, N.x + 2 * N.y])

        Last is DCM (Direction Cosine Matrix). This is a rotation matrix
        given manually.

        >>> B.orient(N, 'DCM', eye(3))
        >>> B.orient(N, 'DCM', ImmutableMatrix([[0, 1, 0], [0, 0, -1], [-1, 0, 0]]))

        """

        from sympy.physics.vector.functions import dynamicsymbols
        _check_frame(parent)

        # Allow passing a rotation matrix manually.
        if rot_type == 'DCM':
            # When rot_type == 'DCM', then amounts must be a Matrix type object
            # (e.g. sympy.matrices.dense.MutableDenseMatrix).
            if not isinstance(amounts, MatrixBase):
                raise TypeError("Amounts must be a sympy Matrix type object.")
        else:
            amounts = list(amounts)
            for i, v in enumerate(amounts):
                if not isinstance(v, Vector):
                    amounts[i] = sympify(v)

        def _rot(axis, angle):
            """DCM for simple axis 1,2,or 3 rotations. """
            if axis == 1:
                return Matrix([[1, 0, 0],
                    [0, cos(angle), -sin(angle)],
                    [0, sin(angle), cos(angle)]])
            elif axis == 2:
                return Matrix([[cos(angle), 0, sin(angle)],
                    [0, 1, 0],
                    [-sin(angle), 0, cos(angle)]])
            elif axis == 3:
                return Matrix([[cos(angle), -sin(angle), 0],
                    [sin(angle), cos(angle), 0],
                    [0, 0, 1]])

        approved_orders = ('123', '231', '312', '132', '213', '321', '121',
                           '131', '212', '232', '313', '323', '')
        rot_order = str(
            rot_order).upper()  # Now we need to make sure XYZ = 123
        rot_type = rot_type.upper()
        rot_order = [i.replace('X', '1') for i in rot_order]
        rot_order = [i.replace('Y', '2') for i in rot_order]
        rot_order = [i.replace('Z', '3') for i in rot_order]
        rot_order = ''.join(rot_order)
        if not rot_order in approved_orders:
            raise TypeError('The supplied order is not an approved type')
        parent_orient = []
        if rot_type == 'AXIS':
            if not rot_order == '':
                raise TypeError('Axis orientation takes no rotation order')
            if not (isinstance(amounts, (list, tuple)) & (len(amounts) == 2)):
                raise TypeError('Amounts are a list or tuple of length 2')
            theta = amounts[0]
            axis = amounts[1]
            axis = _check_vector(axis)
            if not axis.dt(parent) == 0:
                raise ValueError('Axis cannot be time-varying')
            axis = axis.express(parent).normalize()
            axis = axis.args[0][0]
            parent_orient = ((eye(3) - axis * axis.T) * cos(theta) +
                    Matrix([[0, -axis[2], axis[1]], [axis[2], 0, -axis[0]],
                        [-axis[1], axis[0], 0]]) * sin(theta) + axis * axis.T)
        elif rot_type == 'QUATERNION':
            if not rot_order == '':
                raise TypeError(
                    'Quaternion orientation takes no rotation order')
            if not (isinstance(amounts, (list, tuple)) & (len(amounts) == 4)):
                raise TypeError('Amounts are a list or tuple of length 4')
            q0, q1, q2, q3 = amounts
            parent_orient = (Matrix([[q0 ** 2 + q1 ** 2 - q2 ** 2 - q3 **
                2, 2 * (q1 * q2 - q0 * q3), 2 * (q0 * q2 + q1 * q3)],
                [2 * (q1 * q2 + q0 * q3), q0 ** 2 - q1 ** 2 + q2 ** 2 - q3 ** 2,
                2 * (q2 * q3 - q0 * q1)], [2 * (q1 * q3 - q0 * q2), 2 * (q0 *
                q1 + q2 * q3), q0 ** 2 - q1 ** 2 - q2 ** 2 + q3 ** 2]]))
        elif rot_type == 'BODY':
            if not (len(amounts) == 3 & len(rot_order) == 3):
                raise TypeError('Body orientation takes 3 values & 3 orders')
            a1 = int(rot_order[0])
            a2 = int(rot_order[1])
            a3 = int(rot_order[2])
            parent_orient = (_rot(a1, amounts[0]) * _rot(a2, amounts[1])
                    * _rot(a3, amounts[2]))
        elif rot_type == 'SPACE':
            if not (len(amounts) == 3 & len(rot_order) == 3):
                raise TypeError('Space orientation takes 3 values & 3 orders')
            a1 = int(rot_order[0])
            a2 = int(rot_order[1])
            a3 = int(rot_order[2])
            parent_orient = (_rot(a3, amounts[2]) * _rot(a2, amounts[1])
                    * _rot(a1, amounts[0]))
        elif rot_type == 'DCM':
            parent_orient = amounts
        else:
            raise NotImplementedError('That is not an implemented rotation')
        #Reset the _dcm_cache of this frame, and remove it from the _dcm_caches
        #of the frames it is linked to. Also remove it from the _dcm_dict of
        #its parent
        frames = self._dcm_cache.keys()
        dcm_dict_del = []
        dcm_cache_del = []
        for frame in frames:
            if frame in self._dcm_dict:
                dcm_dict_del += [frame]
            dcm_cache_del += [frame]
        for frame in dcm_dict_del:
            del frame._dcm_dict[self]
        for frame in dcm_cache_del:
            del frame._dcm_cache[self]
        #Add the dcm relationship to _dcm_dict
        self._dcm_dict = self._dlist[0] = {}
        self._dcm_dict.update({parent: parent_orient.T})
        parent._dcm_dict.update({self: parent_orient})
        #Also update the dcm cache after resetting it
        self._dcm_cache = {}
        self._dcm_cache.update({parent: parent_orient.T})
        parent._dcm_cache.update({self: parent_orient})
        if rot_type == 'QUATERNION':
            t = dynamicsymbols._t
            q0, q1, q2, q3 = amounts
            q0d = diff(q0, t)
            q1d = diff(q1, t)
            q2d = diff(q2, t)
            q3d = diff(q3, t)
            w1 = 2 * (q1d * q0 + q2d * q3 - q3d * q2 - q0d * q1)
            w2 = 2 * (q2d * q0 + q3d * q1 - q1d * q3 - q0d * q2)
            w3 = 2 * (q3d * q0 + q1d * q2 - q2d * q1 - q0d * q3)
            wvec = Vector([(Matrix([w1, w2, w3]), self)])
        elif rot_type == 'AXIS':
            thetad = (amounts[0]).diff(dynamicsymbols._t)
            wvec = thetad * amounts[1].express(parent).normalize()
        elif rot_type == 'DCM':
            wvec = self._w_diff_dcm(parent)
        else:
            try:
                from sympy.polys.polyerrors import CoercionFailed
                from sympy.physics.vector.functions import kinematic_equations
                q1, q2, q3 = amounts
                u1, u2, u3 = symbols('u1, u2, u3', cls=Dummy)
                templist = kinematic_equations([u1, u2, u3], [q1, q2, q3],
                                               rot_type, rot_order)
                templist = [expand(i) for i in templist]
                td = solve(templist, [u1, u2, u3])
                u1 = expand(td[u1])
                u2 = expand(td[u2])
                u3 = expand(td[u3])
                wvec = u1 * self.x + u2 * self.y + u3 * self.z
            except (CoercionFailed, AssertionError):
                wvec = self._w_diff_dcm(parent)
        self._ang_vel_dict.update({parent: wvec})
        parent._ang_vel_dict.update({self: -wvec})
        self._var_dict = {}
예제 #54
0
from sympy.core.backend import sin, cos, tan, pi, symbols, Matrix, zeros
from sympy.physics.mechanics import (Particle, Point, ReferenceFrame,
                                     RigidBody, Vector)
from sympy.physics.mechanics import (angular_momentum, dynamicsymbols,
                                     inertia, inertia_of_point_mass,
                                     kinetic_energy, linear_momentum,
                                     outer, potential_energy, msubs,
                                     find_dynamicsymbols)

from sympy.physics.mechanics.functions import gravity
from sympy.physics.vector.vector import Vector
from sympy.utilities.pytest import raises

Vector.simp = True
q1, q2, q3, q4, q5 = symbols('q1 q2 q3 q4 q5')
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])


def test_inertia():
    N = ReferenceFrame('N')
    ixx, iyy, izz = symbols('ixx iyy izz')
    ixy, iyz, izx = symbols('ixy iyz izx')
    assert inertia(N, ixx, iyy, izz) == (ixx * (N.x | N.x) + iyy *
            (N.y | N.y) + izz * (N.z | N.z))
    assert inertia(N, 0, 0, 0) == 0 * (N.x | N.x)
    assert inertia(N, ixx, iyy, izz, ixy, iyz, izx) == (ixx * (N.x | N.x) +
            ixy * (N.x | N.y) + izx * (N.x | N.z) + ixy * (N.y | N.x) + iyy *
        (N.y | N.y) + iyz * (N.y | N.z) + izx * (N.z | N.x) + iyz * (N.z |
def test_n_link_pendulum_on_cart_inputs():
    l0, m0 = symbols("l0 m0")
    m1 = symbols("m1")
    g = symbols("g")
    q0, q1, F, T1 = dynamicsymbols("q0 q1 F T1")
    u0, u1 = dynamicsymbols("u0 u1")

    kane1 = models.n_link_pendulum_on_cart(1)
    massmatrix1 = Matrix([[m0 + m1, -l0*m1*cos(q1)],
                          [-l0*m1*cos(q1), l0**2*m1]])
    forcing1 = Matrix([[-l0*m1*u1**2*sin(q1) + F], [g*l0*m1*sin(q1)]])
    assert simplify(massmatrix1 - kane1.mass_matrix) == zeros(2)
    assert simplify(forcing1 - kane1.forcing) == Matrix([0, 0])

    kane2 = models.n_link_pendulum_on_cart(1, False)
    massmatrix2 = Matrix([[m0 + m1, -l0*m1*cos(q1)],
                          [-l0*m1*cos(q1), l0**2*m1]])
    forcing2 = Matrix([[-l0*m1*u1**2*sin(q1)], [g*l0*m1*sin(q1)]])
    assert simplify(massmatrix2 - kane2.mass_matrix) == zeros(2)
    assert simplify(forcing2 - kane2.forcing) == Matrix([0, 0])

    kane3 = models.n_link_pendulum_on_cart(1, False, True)
    massmatrix3 = Matrix([[m0 + m1, -l0*m1*cos(q1)],
                          [-l0*m1*cos(q1), l0**2*m1]])
    forcing3 = Matrix([[-l0*m1*u1**2*sin(q1)], [g*l0*m1*sin(q1) + T1]])
    assert simplify(massmatrix3 - kane3.mass_matrix) == zeros(2)
    assert simplify(forcing3 - kane3.forcing) == Matrix([0, 0])

    kane4 = models.n_link_pendulum_on_cart(1, True, False)
    massmatrix4 = Matrix([[m0 + m1, -l0*m1*cos(q1)],
                          [-l0*m1*cos(q1), l0**2*m1]])
    forcing4 = Matrix([[-l0*m1*u1**2*sin(q1) + F], [g*l0*m1*sin(q1)]])
    assert simplify(massmatrix4 - kane4.mass_matrix) == zeros(2)
    assert simplify(forcing4 - kane4.forcing) == Matrix([0, 0])