Пример #1
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]]))
Пример #2
1
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([])
Пример #3
1
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)
    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)
    def __init__(self):
        #We define some quantities required for tests here..
        self.p = dynamicsymbols('p:3')
        self.q = dynamicsymbols('q:3')
        self.dynamic = list(self.p) + list(self.q)
        self.states = [radians(45) for x in self.p] + \
                               [radians(30) for x in self.q]

        self.I = ReferenceFrame('I')
        self.A = self.I.orientnew('A', 'space', self.p, 'XYZ')
        self.B = self.A.orientnew('B', 'space', self.q, 'XYZ')

        self.O = Point('O')
        self.P1 = self.O.locatenew('P1', 10 * self.I.x + \
                                      10 * self.I.y + 10 * self.I.z)
        self.P2 = self.P1.locatenew('P2', 10 * self.I.x + \
                                    10 * self.I.y + 10 * self.I.z)

        self.point_list1 = [[2, 3, 1], [4, 6, 2], [5, 3, 1], [5, 3, 6]]
        self.point_list2 = [[3, 1, 4], [3, 8, 2], [2, 1, 6], [2, 1, 1]]

        self.shape1 = Cylinder()
        self.shape2 = Cylinder()


        self.Ixx, self.Iyy, self.Izz = symbols('Ixx Iyy Izz')
        self.mass = symbols('mass')
        self.parameters = [self.Ixx, self.Iyy, self.Izz, self.mass]
        self.param_vals = [0, 0, 0, 0]

        self.inertia = inertia(self.A, self.Ixx, self.Iyy, self.Izz)

        self.rigid_body = RigidBody('rigid_body1', self.P1, self.A, \
                                 self.mass, (self.inertia, self.P1))

        self.global_frame1 = VisualizationFrame('global_frame1', \
                                self.A, self.P1, self.shape1)

        self.global_frame2 = VisualizationFrame('global_frame2', \
                                self.B, self.P2, self.shape2)

        self.scene1 = Scene(self.I, self.O, \
                            (self.global_frame1, self.global_frame2), \
                                             name='scene')

        self.particle = Particle('particle1', self.P1, self.mass)

        #To make it more readable
        p = self.p
        q = self.q
        #Here is the dragon ..
        self.transformation_matrix = \
            [[cos(p[1])*cos(p[2]), sin(p[2])*cos(p[1]), -sin(p[1]), 0], \
             [sin(p[0])*sin(p[1])*cos(p[2]) - sin(p[2])*cos(p[0]), \
                  sin(p[0])*sin(p[1])*sin(p[2]) + cos(p[0])*cos(p[2]), \
                  sin(p[0])*cos(p[1]), 0], \
             [sin(p[0])*sin(p[2]) + sin(p[1])*cos(p[0])*cos(p[2]), \
                 -sin(p[0])*cos(p[2]) + sin(p[1])*sin(p[2])*cos(p[0]), \
                  cos(p[0])*cos(p[1]), 0], \
             [10, 10, 10, 1]]
Пример #5
1
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])
Пример #6
1
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.mass = m
    pa.point = P
    BL = [pa]

    KM = Kane(N)
    KM.coords([q])
    KM.speeds([u])
    KM.kindiffeq(kd)
    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))
Пример #7
1
def test_one_dof():
    # This is for a 1 dof spring-mass-damper case.
    # It is described in more detail in the Kane 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.mass = m
    pa.point = P
    BL = [pa]

    KM = Kane(N)
    KM.coords([q])
    KM.speeds([u])
    KM.kindiffeq(kd)
    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 KM.linearize() == (Matrix([[0, 1], [k, c]]), Matrix([]))
Пример #8
1
def test_dyadic():
    d1 = A.x | A.x
    d2 = A.y | A.y
    d3 = A.x | A.y
    assert d1 * 0 == 0
    assert d1 != 0
    assert d1 * 2 == 2 * A.x | A.x
    assert d1 / 2. == 0.5 * d1
    assert d1 & (0 * d1) == 0
    assert d1 & d2 == 0
    assert d1 & A.x == A.x
    assert d1 ^ A.x == 0
    assert d1 ^ A.y == A.x | A.z
    assert d1 ^ A.z == - A.x | A.y
    assert d2 ^ A.x == - A.y | A.z
    assert A.x ^ d1 == 0
    assert A.y ^ d1 == - A.z | A.x
    assert A.z ^ d1 == A.y | A.x
    assert A.x & d1 == A.x
    assert A.y & d1 == 0
    assert A.y & d2 == A.y
    assert d1 & d3 == A.x | A.y
    assert d3 & d1 == 0
    assert d1.dt(A) == 0
    q = dynamicsymbols('q')
    qd = dynamicsymbols('q', 1)
    B = A.orientnew('B', 'Axis', [q, A.z])
    assert d1.express(B) == d1.express(B, B)
    assert d1.express(B) == ((cos(q)**2) * (B.x | B.x) + (-sin(q) * cos(q)) *
            (B.x | B.y) + (-sin(q) * cos(q)) * (B.y | B.x) + (sin(q)**2) *
            (B.y | B.y))
    assert d1.express(B, A) == (cos(q)) * (B.x | A.x) + (-sin(q)) * (B.y | A.x)
    assert d1.express(A, B) == (cos(q)) * (A.x | B.x) + (-sin(q)) * (A.x | B.y)
    assert d1.dt(B) == (-qd) * (A.y | A.x) + (-qd) * (A.x | A.y)
Пример #9
1
def test_parallel_axis():
    # This is for a 2 dof inverted pendulum on a cart.
    # This tests the parallel axis code in Kane. 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=Kane(N)
    km.coords([q1, q2])
    km.speeds([u1, u2])
    km.kindiffeq(kindiffs)
    (fr,frstar) = km.kanes_equations(forceList, bodyList)
    mm = km.mass_matrix_full
    assert mm[3, 3] == -Iz
Пример #10
1
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 Kane. 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])
    R.set_ang_vel(N, u1 * L.x + u2 * L.y + u3 * L.z)
    R.set_ang_acc(N, R.ang_vel_in(N).dt(R) + (R.ang_vel_in(N) ^
        R.ang_vel_in(N)))

    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 = [q1d - u3/cos(q3), q2d - u1, q3d - u2 + u3 * tan(q2)]

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

    KM = Kane(N)
    KM.coords([q1, q2, q3])
    KM.speeds([u1, u2, u3, u4, u5])
    KM.kindiffeq(kd)
    kdd = KM.kindiffdict()
    (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 = Kane(N)
    KM2.coords([q1, q2, q3])
    KM2.speeds([u1, u2, u3], u_auxiliary=[u4, u5])
    KM2.kindiffeq(kd)
    (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})

    assert fr.expand() == fr2.expand()
    assert frstar.expand() == frstar2.expand()
Пример #11
1
    def __init__(self):

        super(Aircraft, self).__init__()

        # states
        body__x, body__v_x, body__a_x = mech.dynamicsymbols('body.x, body.v_x, body.a_x')
        self.x = sympy.Matrix([body__x, body__v_x, body__a_x])
        self.x0 = {
            body__x : 0.0,
            body__v_x : 0.0,
            body__a_x : 0.0,
            }

        # variables
        accel__ma_x, accel__b_x__y = mech.dynamicsymbols('accel.ma_x, accel.b_x.y')
        self.v = sympy.Matrix([accel__ma_x, accel__b_x__y])

        # constants
        self.c = sympy.Matrix([])
        self.c0 = {
            }

        # parameters
        body__g, body__c, body__m, accel__b_x__b = sympy.symbols('body.g, body.c, body.m, accel.b_x.b')
        self.p = sympy.Matrix([body__g, body__c, body__m, accel__b_x__b])
        self.p0 = {
            body__g : 9.81,
            body__c : 0.9,
            body__m : 1.0,
            accel__b_x__b : 0.0,
            }

        # inputs
        body__f_x, accel__a_x, accel__b_x__u = mech.dynamicsymbols('body.f_x, accel.a_x, accel.b_x.u')
        self.u = sympy.Matrix([body__f_x, accel__a_x, accel__b_x__u])
        self.u0 = {
            body__f_x : 0.0,
            accel__a_x : 0.0,
            accel__b_x__u : 0.0,
            }

        # outputs
        body__x, body__v_x, body__a_x, accel__ma_x, accel__b_x__y = mech.dynamicsymbols('body.x, body.v_x, body.a_x, accel.ma_x, accel.b_x.y')
        self.y = sympy.Matrix([body__x, body__v_x, body__a_x, accel__ma_x, accel__b_x__y])

        # equations
        self.eqs = [
            body__a_x - (accel__a_x),
            (body__x).diff(self.t) - (body__v_x),
            (body__v_x).diff(self.t) - (body__a_x),
            body__f_x - (body__m * body__a_x),
            accel__b_x__u - (accel__a_x),
            accel__ma_x - (accel__b_x__y),
            accel__b_x__y - (accel__b_x__u + accel__b_x__b),
            ]

        self.compute_fg()
Пример #12
1
def test_kin_eqs():
    q0, q1, q2, q3 = dynamicsymbols('q0 q1 q2 q3')
    q0d, q1d, q2d, q3d = dynamicsymbols('q0 q1 q2 q3', 1)
    u1, u2, u3 = dynamicsymbols('u1 u2 u3')
    kds = kinematic_equations([u1, u2, u3], [q0, q1, q2, q3], 'quaternion')
    assert kds == [-0.5 * q0 * u1 - 0.5 * q2 * u3 + 0.5 * q3 * u2 + q1d,
            -0.5 * q0 * u2 + 0.5 * q1 * u3 - 0.5 * q3 * u1 + q2d,
            -0.5 * q0 * u3 - 0.5 * q1 * u2 + 0.5 * q2 * u1 + q3d,
            0.5 * q1 * u1 + 0.5 * q2 * u2 + 0.5 * q3 * u3 + q0d]
Пример #13
1
    def test_integrate(self):

        times = np.linspace(0, 1, 100)

        # Try without calling generate_ode_function.
        # ------------------------------------------
        sys = System(self.kane, times=times)
        x_01 = sys.integrate()

        sys = System(self.kane, times=times)
        sys.generate_ode_function(generator='lambdify')
        x_02 = sys.integrate()

        testing.assert_allclose(x_01, x_02)

        # Ensure that the defaults are as expected.
        # -----------------------------------------
        constants_dict = dict(zip(sm.symbols('m0, k0, c0, g'),
                                  [1.0, 1.0, 1.0, 1.0]))
        specified_dict = {dynamicsymbols('f0'): 0.0}
        x_03 = sys.ode_solver(sys.evaluate_ode_function, [0, 0], sys.times,
                              args=(specified_dict, constants_dict))
        testing.assert_allclose(x_02, x_03)

        # Ensure that initial conditions are reordered properly.
        # ------------------------------------------------------
        sys = System(self.kane, times=times)
        # I know that this is the order of the states.
        x0 = [5.1, 3.7]
        ic = {dynamicsymbols('x0'): x0[0], dynamicsymbols('v0'): x0[1]}
        sys.initial_conditions = ic
        x_04 = sys.integrate()
        x_05 = sys.ode_solver(
            sys.evaluate_ode_function, x0, sys.times,
            args=(sys._specifieds_padded_with_defaults(),
                  sys._constants_padded_with_defaults()))

        testing.assert_allclose(x_04, x_05)

        # Test a generator other than lambdify.
        # -------------------------------------
        if theano:
            sys.generate_ode_function(generator='theano')
            sys.times = times
            x_06 = sys.integrate()
            testing.assert_allclose(x_04, x_06)
        else:
            warnings.warn("Theano was not found so the related tests are being"
                          " skipped.", PyDyImportWarning)

        # Unrecognized generator.
        # -----------------------
        sys = System(self.kane, times=times)
        with testing.assert_raises(NotImplementedError):
            sys.generate_ode_function(generator='made-up')
Пример #14
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
Пример #15
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])
Пример #16
0
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)
Пример #17
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 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)
Пример #18
0
def test_get_motion_methods():
    #Initialization
    t = dynamicsymbols._t
    s1, s2, s3 = symbols('s1 s2 s3')
    S1, S2, S3 = symbols('S1 S2 S3')
    S4, S5, S6 = symbols('S4 S5 S6')
    t1, t2 = symbols('t1 t2')
    a, b, c = dynamicsymbols('a b c')
    ad, bd, cd = dynamicsymbols('a b c', 1)
    a2d, b2d, c2d = dynamicsymbols('a b c', 2)
    v0 = S1*N.x + S2*N.y + S3*N.z
    v01 = S4*N.x + S5*N.y + S6*N.z
    v1 = s1*N.x + s2*N.y + s3*N.z
    v2 = a*N.x + b*N.y + c*N.z
    v2d = ad*N.x + bd*N.y + cd*N.z
    v2dd = a2d*N.x + b2d*N.y + c2d*N.z
    #Test position parameter
    assert get_motion_params(frame = N) == (0, 0, 0)
    assert get_motion_params(N, position=v1) == (0, 0, v1)
    assert get_motion_params(N, position=v2) == (v2dd, v2d, v2)
    #Test velocity parameter
    assert get_motion_params(N, velocity=v1) == (0, v1, v1 * t)
    assert get_motion_params(N, velocity=v1, position=v0, timevalue1=t1) == \
           (0, v1, v0 + v1*(t - t1))
    answer = get_motion_params(N, velocity=v1, position=v2, timevalue1=t1)
    answer_expected = (0, v1, v1*t - v1*t1 + v2.subs(t, t1))
    assert answer == answer_expected

    answer = get_motion_params(N, velocity=v2, position=v0, timevalue1=t1)
    integral_vector = Integral(a, (t, t1, t))*N.x + Integral(b, (t, t1, t))*N.y \
            + Integral(c, (t, t1, t))*N.z
    answer_expected = (v2d, v2, v0 + integral_vector)
    assert answer == answer_expected

    #Test acceleration parameter
    assert get_motion_params(N, acceleration=v1) == (v1, v1 * t, v1 * t**2/2)
    assert get_motion_params(N, acceleration=v1, velocity=v0,
                          position=v2, timevalue1=t1, timevalue2=t2) == \
           (v1, (v0 + v1*t - v1*t2),
            -v0*t1 + v1*t**2/2 + v1*t2*t1 - \
            v1*t1**2/2 + t*(v0 - v1*t2) + \
            v2.subs(t, t1))
    assert get_motion_params(N, acceleration=v1, velocity=v0,
                             position=v01, timevalue1=t1, timevalue2=t2) == \
           (v1, v0 + v1*t - v1*t2,
            -v0*t1 + v01 + v1*t**2/2 + \
            v1*t2*t1 - v1*t1**2/2 + \
            t*(v0 - v1*t2))
    answer = get_motion_params(N, acceleration=a*N.x, velocity=S1*N.x,
                          position=S2*N.x, timevalue1=t1, timevalue2=t2)
    i1 = Integral(a, (t, t2, t))
    answer_expected = (a*N.x, (S1 + i1)*N.x, \
        (S2 + Integral(S1 + i1, (t, t1, t)))*N.x)
    assert answer == answer_expected
Пример #19
0
def test_point_a2pt_theorys():
    q = dynamicsymbols('q')
    qd = dynamicsymbols('q', 1)
    qdd = dynamicsymbols('q', 2)
    N = ReferenceFrame('N')
    B = N.orientnew('B', 'Axis', [q, N.z])
    O = Point('O')
    P = O.locatenew('P', 0)
    O.set_vel(N, 0)
    assert P.a2pt_theory(O, N, B) == 0
    P.set_pos(O, B.x)
    assert P.a2pt_theory(O, N, B) == (-qd**2) * B.x + (qdd) * B.y
Пример #20
0
def test_dyadic():
    d1 = A.x | A.x
    d2 = A.y | A.y
    d3 = A.x | A.y
    assert d1 * 0 == 0
    assert d1 != 0
    assert d1 * 2 == 2 * A.x | A.x
    assert d1 / 2. == 0.5 * d1
    assert d1 & (0 * d1) == 0
    assert d1 & d2 == 0
    assert d1 & A.x == A.x
    assert d1 ^ A.x == 0
    assert d1 ^ A.y == A.x | A.z
    assert d1 ^ A.z == - A.x | A.y
    assert d2 ^ A.x == - A.y | A.z
    assert A.x ^ d1 == 0
    assert A.y ^ d1 == - A.z | A.x
    assert A.z ^ d1 == A.y | A.x
    assert A.x & d1 == A.x
    assert A.y & d1 == 0
    assert A.y & d2 == A.y
    assert d1 & d3 == A.x | A.y
    assert d3 & d1 == 0
    assert d1.dt(A) == 0
    q = dynamicsymbols('q')
    qd = dynamicsymbols('q', 1)
    B = A.orientnew('B', 'Axis', [q, A.z])
    assert d1.express(B) == d1.express(B, B)
    assert d1.express(B) == ((cos(q)**2) * (B.x | B.x) + (-sin(q) * cos(q)) *
            (B.x | B.y) + (-sin(q) * cos(q)) * (B.y | B.x) + (sin(q)**2) *
            (B.y | B.y))
    assert d1.express(B, A) == (cos(q)) * (B.x | A.x) + (-sin(q)) * (B.y | A.x)
    assert d1.express(A, B) == (cos(q)) * (A.x | B.x) + (-sin(q)) * (A.x | B.y)
    assert d1.dt(B) == (-qd) * (A.y | A.x) + (-qd) * (A.x | A.y)

    assert d1.to_matrix(A) == Matrix([[1, 0, 0], [0, 0, 0], [0, 0, 0]])
    assert d1.to_matrix(A, B) == Matrix([[cos(q), -sin(q), 0],
                                         [0, 0, 0],
                                         [0, 0, 0]])
    assert d3.to_matrix(A) == Matrix([[0, 1, 0], [0, 0, 0], [0, 0, 0]])
    a, b, c, d, e, f = symbols('a, b, c, d, e, f')
    v1 = a * A.x + b * A.y + c * A.z
    v2 = d * A.x + e * A.y + f * A.z
    d4 = v1.outer(v2)
    assert d4.to_matrix(A) == Matrix([[a * d, a * e, a * f],
                                      [b * d, b * e, b * f],
                                      [c * d, c * e, c * f]])
    d5 = v1.outer(v1)
    C = A.orientnew('C', 'Axis', [q, A.x])
    for expected, actual in zip(C.dcm(A) * d5.to_matrix(A) * C.dcm(A).T,
                                d5.to_matrix(C)):
        assert (expected - actual).simplify() == 0
Пример #21
0
    def test_integrate(self):

        times = np.linspace(0, 1, 100)

        # Try without calling generate_ode_function.
        # ------------------------------------------
        sys = System(self.kane, times=times)
        x_01 = sys.integrate()

        sys = System(self.kane, times=times)
        rhs = sys.generate_ode_function(generator='lambdify')
        x_02 = sys.integrate()

        testing.assert_allclose(x_01, x_02)

        # Ensure that the defaults are as expected.
        # -----------------------------------------
        constants_dict = dict(zip(symbols('m, k, c, g'), [1.0, 1.0, 1.0, 1.0]))
        specified_dict = {dynamicsymbols('F'): 0.0}
        x_03 = sys.ode_solver(sys.evaluate_ode_function, [0, 0], sys.times,
                args=({
                    'constants': constants_dict,
                    'specified': specified_dict},))
        testing.assert_allclose(x_02, x_03)

        # Ensure that initial conditions are reordered properly.
        # ------------------------------------------------------
        sys = System(self.kane, times=times)
        # I know that this is the order of the states.
        x0 = [5.1, 3.7]
        ic = {dynamicsymbols('x'): x0[0], dynamicsymbols('v'): x0[1]}
        sys.initial_conditions = ic
        x_04 = sys.integrate()
        x_05 = sys.ode_solver(sys.evaluate_ode_function,
                x0, sys.times, args=(
                    {'constants': sys._constants_padded_with_defaults(),
                     'specified': sys._specifieds_padded_with_defaults()},))

        testing.assert_allclose(x_04, x_05)

        # Test a generator other than lambdify.
        # -------------------------------------
        sys.generate_ode_function(generator='theano')
        sys.times = times
        x_06 = sys.integrate()
        testing.assert_allclose(x_04, x_06)

        # Unrecognized generator.
        # -----------------------
        sys = System(self.kane, times=times)
        with testing.assert_raises(NotImplementedError):
            sys.generate_ode_function(generator='made-up')
Пример #22
0
def test_point_v2pt_theorys():
    q = dynamicsymbols('q')
    qd = dynamicsymbols('q', 1)
    N = ReferenceFrame('N')
    B = N.orientnew('B', 'Axis', [q, N.z])
    O = Point('O')
    P = O.locatenew('P', 0)
    O.set_vel(N, 0)
    assert P.v2pt_theory(O, N, B) == 0
    P = O.locatenew('P', B.x)
    assert P.v2pt_theory(O, N, B) == (qd * B.z ^ B.x)
    O.set_vel(N, N.x)
    assert P.v2pt_theory(O, N, B) == N.x + qd * B.y
Пример #23
0
# -*- coding: utf-8 -*-
"""
Spyder Editor

Author: Chul Lee
"""
import scipy as sci
import numpy as np
#import plot.matplotlib as plt
import sympy as sym
from sympy.physics.mechanics import dynamicsymbols
"""Question 1"""
t, u, v, w = dynamicsymbols(
    't u v w')  #declares t u v w as symbols for differentiation
t, u, v, w
var = ['t', 'u', 'v', 'w']
#z = [1,1,1,1] #initial guess
#t=z[0]
#u=z[1]
#v=z[2]
#w=z[3]

#vec = np.array(z) #sets up vector
#vec[0]=t**4+u**4-1
#vec[1]=t**2-u**2+1
#vec[2]=v**4+w**4-1
#vec[3]=v**2-w**2+1
f1 = t**4 + u**4 - 1
f2 = t**2 - u**2 + 1
f3 = v**4 + w**4 - 1
f4 = v**2 - w**2 + 1
Пример #24
0
#!/usr/bin/env python
# -*- coding: utf-8 -*-
"""Exercise 8.6 from Kane 1985."""

from __future__ import division
from sympy import cos, sin, solve, simplify, symbols
from sympy.physics.mechanics import ReferenceFrame, Point
from sympy.physics.mechanics import dynamicsymbols
from util import msprint, subs, partial_velocities, generalized_active_forces

## --- Declare symbols ---
# Define the system with 6 generalized speeds as follows:
q1, q2, q3 = dynamicsymbols('q1:4')
q1d, q2d, q3d = dynamicsymbols('q1:4', level=1)
u1, u2, u3 = dynamicsymbols('u1:4')
L1, L2, L3, L4 = symbols('L1:5')
g, m1, m2, t = symbols('g m1 m2 t')

# --- ReferenceFrames ---
A = ReferenceFrame('A')

# --- Define Points and set their velocities ---
pO = Point('O')
pO.set_vel(A, 0)
pP1 = pO.locatenew('P1', L1 * (cos(q1) * A.x + sin(q1) * A.y))
pP1.set_vel(A, pP1.pos_from(pO).diff(t, A))
pP2 = pP1.locatenew('P2', L2 * (cos(q2) * A.x + sin(q2) * A.y))
pP2.set_vel(A, pP2.pos_from(pO).diff(t, A))

## --- configuration constraints ---
cc = [
Пример #25
0
#!/usr/bin/env python
# coding: utf-8

# In[295]:

#Escrito por Alfredo Aguiar 30 de diciembre del 2020.

# importamos las librerías necesarias
import sympy as sp  # librería para cálculo simbólico

from sympy.physics.vector import init_vprinting

init_vprinting(use_latex='mathjax', pretty_print=False)
from sympy.physics.mechanics import dynamicsymbols

theta1, theta2, l1, l2, theta, alpha, a, d = dynamicsymbols(
    'theta1 theta2 l1 l2 theta alpha a d')

#Definimos las matrices de rotacion y traslacion correspondientes

#Rotacion en Z
rz = sp.Matrix([[sp.cos(theta), -sp.sin(theta), 0, 0],
                [sp.sin(theta), sp.cos(theta), 0, 0], [0, 0, 1, 0],
                [0, 0, 0, 1]])

#Traslacion en Z
tz = sp.Matrix([[1, 0, 0, 0], [0, 1, 0, 0], [0, 0, 1, d], [0, 0, 0, 1]])

#Traslacion en X
tx = sp.Matrix([[1, 0, 0, a], [0, 1, 0, 0], [0, 0, 1, 0], [0, 0, 0, 1]])

#Rotacion en X
Пример #26
0
import sympy.physics.mechanics as me
import sympy as sm
import math as m
import numpy as np

x, y = me.dynamicsymbols('x y')
xd, yd = me.dynamicsymbols('x y', 1)
e1 = (x + y)**2 + (x - y)**3
e2 = (x - y)**2
e3 = x**2 + y**2 + 2 * x * y
m1 = sm.Matrix([e1, e2]).reshape(2, 1)
m2 = sm.Matrix([(x + y)**2, (x - y)**2]).reshape(1, 2)
m3 = m1 + sm.Matrix([x, y]).reshape(2, 1)
am = sm.Matrix([i.expand() for i in m1]).reshape((m1).shape[0], (m1).shape[1])
cm = sm.Matrix([
    i.expand() for i in sm.Matrix([(x + y)**2, (x - y)**2]).reshape(1, 2)
]).reshape((sm.Matrix([(x + y)**2, (x - y)**2]).reshape(1, 2)).shape[0],
           (sm.Matrix([(x + y)**2, (x - y)**2]).reshape(1, 2)).shape[1])
em = sm.Matrix([i.expand() for i in m1 + sm.Matrix([x, y]).reshape(2, 1)
                ]).reshape((m1 + sm.Matrix([x, y]).reshape(2, 1)).shape[0],
                           (m1 + sm.Matrix([x, y]).reshape(2, 1)).shape[1])
f = (e1).expand()
g = (e2).expand()
a = sm.factor((e3), x)
bm = sm.Matrix([sm.factor(i, x) for i in m1]).reshape((m1).shape[0],
                                                      (m1).shape[1])
cm = sm.Matrix([sm.factor(i, x) for i in m1 + sm.Matrix([x, y]).reshape(2, 1)
                ]).reshape((m1 + sm.Matrix([x, y]).reshape(2, 1)).shape[0],
                           (m1 + sm.Matrix([x, y]).reshape(2, 1)).shape[1])
a = (e3).diff(x)
b = (e3).diff(y)
Пример #27
0
import sympy.physics.mechanics as me
import sympy as sm
import math as m
import numpy as np

x1, x2 = me.dynamicsymbols("x1 x2")
f1 = x1 * x2 + 3 * x1**2
f2 = x1 * me.dynamicsymbols._t + x2 * me.dynamicsymbols._t**2
x, y = me.dynamicsymbols("x y")
xd, yd = me.dynamicsymbols("x y", 1)
yd2 = me.dynamicsymbols("y", 2)
q1, q2, q3, u1, u2 = me.dynamicsymbols("q1 q2 q3 u1 u2")
p1, p2 = me.dynamicsymbols("p1 p2")
p1d, p2d = me.dynamicsymbols("p1 p2", 1)
w1, w2, w3, r1, r2 = me.dynamicsymbols("w1 w2 w3 r1 r2")
w1d, w2d, w3d, r1d, r2d = me.dynamicsymbols("w1 w2 w3 r1 r2", 1)
r1d2, r2d2 = me.dynamicsymbols("r1 r2", 2)
c11, c12, c21, c22 = me.dynamicsymbols("c11 c12 c21 c22")
d11, d12, d13 = me.dynamicsymbols("d11 d12 d13")
j1, j2 = me.dynamicsymbols("j1 j2")
n = sm.symbols("n")
n = sm.I
Пример #28
0
def test_linearize_pendulum_kane_nonminimal():
    # Create generalized coordinates and speeds for this non-minimal realization
    # q1, q2 = N.x and N.y coordinates of pendulum
    # u1, u2 = N.x and N.y velocities of pendulum
    q1, q2 = dynamicsymbols('q1:3')
    q1d, q2d = dynamicsymbols('q1:3', level=1)
    u1, u2 = dynamicsymbols('u1:3')
    u1d, u2d = dynamicsymbols('u1: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])

    # Locate the pendulum mass
    P = pN.locatenew('P1', q1 * N.x + q2 * N.y)
    pP = Particle('pP', P, m)

    # Calculate the kinematic differential equations
    kde = Matrix([q1d - u1, q2d - u2])
    dq_dict = solve(kde, [q1d, q2d])

    # Set velocity of point P
    P.set_vel(N, P.pos_from(pN).dt(N).subs(dq_dict))

    # Configuration constraint is length of pendulum
    f_c = Matrix([P.pos_from(pN).magnitude() - L])

    # Velocity constraint is that the velocity in the A.x direction is
    # always zero (the pendulum is never getting longer).
    f_v = Matrix([P.vel(N).express(A).dot(A.x)])
    f_v.simplify()

    # Acceleration constraints is the time derivative of the velocity constraint
    f_a = f_v.diff(t)
    f_a.simplify()

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

    # Derive the equations of motion using the KanesMethod class.
    KM = KanesMethod(N,
                     q_ind=[q2],
                     u_ind=[u2],
                     q_dependent=[q1],
                     u_dependent=[u1],
                     configuration_constraints=f_c,
                     velocity_constraints=f_v,
                     acceleration_constraints=f_a,
                     kd_eqs=kde)
    (fr, frstar) = KM.kanes_equations([(P, R)], [pP])

    # Set the operating point to be straight down, and non-moving
    q_op = {q1: L, q2: 0}
    u_op = {u1: 0, u2: 0}
    ud_op = {u1d: 0, u2d: 0}

    A, B, inp_vec = KM.linearize(op_point=[q_op, u_op, ud_op],
                                 A_and_B=True,
                                 new_method=True,
                                 simplify=True)

    assert A == Matrix([[0, 1], [-9.8 / L, 0]])
    assert B == Matrix([])
Пример #29
0
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 three
    # speed variables are need to describe this system, along with the disc's
    # mass and radius, and the local gravity (note that mass will drop out).
    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)
    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])
    R.set_ang_vel(N, u1 * L.x + u2 * L.y + u3 * L.z)
    R.set_ang_acc(N,
                  R.ang_vel_in(N).dt(R) + (R.ang_vel_in(N) ^ R.ang_vel_in(N)))

    # 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)
    Dmc.a2pt_theory(C, N, R)

    # This is a simple way to form the inertia dyadic.
    I = inertia(L, m / 4 * r**2, m / 2 * r**2, m / 4 * r**2)

    # Kinematic differential equations; how the generalized coordinate time
    # derivatives relate to generalized speeds.
    kd = [q1d - u3 / cos(q2), q2d - u1, q3d - u2 + u3 * tan(q2)]

    # Creation of the force list; it is the gravitational force at the mass
    # center of the disc. Then we create the disc by assigning a Point to the
    # center of mass attribute, a ReferenceFrame to the frame attribute, and mass
    # and inertia. Then we form the body list.
    ForceList = [(Dmc, -m * g * Y.z)]
    BodyD = RigidBody('BodyD', Dmc, R, m, (I, Dmc))
    BodyList = [BodyD]

    # Finally we form the equations of motion, using the same steps we did
    # before. Specify inertial frame, supply generalized speeds, supply
    # kinematic differential equation dictionary, compute Fr from the force
    # list and Fr* from the body list, compute the mass matrix and forcing
    # terms, then solve for the u dots (time derivatives of the generalized
    # speeds).
    KM = KanesMethod(N, q_ind=[q1, q2, q3], u_ind=[u1, u2, u3], kd_eqs=kd)
    KM.kanes_equations(ForceList, BodyList)
    MM = KM.mass_matrix
    forcing = KM.forcing
    rhs = MM.inv() * forcing
    kdd = KM.kindiffdict()
    rhs = rhs.subs(kdd)
    assert rhs.expand() == Matrix([
        (10 * u2 * u3 * r - 5 * u3**2 * r * tan(q2) + 4 * g * sin(q2)) /
        (5 * r), -2 * u1 * u3 / 3, u1 * (-2 * u2 + u3 * tan(q2))
    ]).expand()
Пример #30
0
import sympy.physics.mechanics as me
import sympy as sm
import math as m
import numpy as np

frame_a = me.ReferenceFrame('a')
frame_b = me.ReferenceFrame('b')
q1, q2, q3 = me.dynamicsymbols('q1 q2 q3')
frame_b.orient(frame_a, 'Axis', [q3, frame_a.x])
dcm = frame_a.dcm(frame_b)
m = dcm * 3 - frame_a.dcm(frame_b)
r = me.dynamicsymbols('r')
circle_area = sm.pi * r**2
u, a = me.dynamicsymbols('u a')
x, y = me.dynamicsymbols('x y')
s = u * me.dynamicsymbols._t - 1 / 2 * a * me.dynamicsymbols._t**2
expr1 = 2 * a * 0.5 - 1.25 + 0.25
expr2 = -1 * x**2 + y**2 + 0.25 * (x + y)**2
expr3 = 0.5 * 10**(-10)
dyadic = me.outer(frame_a.x, frame_a.x) + me.outer(
    frame_a.y, frame_a.y) + me.outer(frame_a.z, frame_a.z)
Пример #31
0
# # Dynamic Model of a Cart with a (not-inverted) pendulum (2D)

# +
from sympy import *
from sympy.physics.mechanics import dynamicsymbols, init_vprinting

init_vprinting()

# Time and input variable
t, u = symbols('t u')

# Physical parameters
m, M, l, g = symbols('m M l g')

# Load angle
alpha = dynamicsymbols('\talpha')

# Angular Velocity
alphadot = diff(alpha, t)

# Angular Acceleration
alphaddot = diff(alphadot, t)
# -

# ## Coordinates $(x,\alpha)$

# +
# Position
x = dynamicsymbols('x')
xL = x - l * sin(alpha)  # xL as function of x
zL = -l * cos(alpha)  # zL as function of z
Пример #32
0
             potential_energy, LagrangesMethod, mechanics_printing)
from sympy.utilities.lambdify import lambdify
from sympy.simplify import trigsimp

import parameters as par

# For shorter expressions
mechanics_printing(pretty_print=False)

# ----------------------------------------------------------------------------
#                                Define symbols
# ----------------------------------------------------------------------------

# Generalized coordinates
n_coords = 3
q = dynamicsymbols("q:" + str(n_coords))
dq = dynamicsymbols("q:" + str(n_coords), level=1)

# Mass and link lengths
m_links = symbols("m_l:2")
d_links = symbols("d_l:2")
m_point = symbols("m_cart m_A m_B m_C")  # for point objects

b_cart, b_joint = symbols("b_cart b_joint")  # Viscous damping

g, t = symbols("g, t")  # General parameters
k, l0 = symbols("k l0")  # Spring parameters
F = symbols("F")  # Input force; both motor and hydraulic brake

# Lists to store the objects
particles = []
Пример #33
0
def main():
    # model_name = 'F2T0RNA'
    #model_name = 'F2T0RNA_fnd'
    # model_name = 'F2T0N0S1_fnd'
    # model_name = 'F2T1RNA'
    # model_name = 'F2T1RNA_fnd'
    # model_name = 'F3T1RNA_fnd'
    # model_name = 'F5T1RNA_fnd'
    # model_name = 'F2T1N0S1_fnd'
    # model_name = 'F0T2RNA'
    # model_name = 'F0T1RNA'
    # model_name = 'F0T2N0S1'
    # model_name = 'F6T0RNA'
    #model_name = 'F5T0N0S1_fnd'
    #
    # model_name = 'F6T0RNA_fnd'
    # model_name = 'F6T0N0S1'
    # model_name = 'F6T1RNA'
    #model_name = 'F6T1RNA_fnd'
    # model_name = 'F6T1N0S1'
    # model_name = 'F6T1N0S1_fnd'
    # model_name = 'F6T2N0S1'

    #model_name='F000101T0N0S1_fnd'
    # model_name='F000111T0N0S1_fnd'
    # model_name='F000111T0RNA_fnd'

    # model_name='F000101T0RNA_fnd'
    # model_name='B000101'
    # model_name='B000011'
    # model_name='B000111'

    # Models=['F2T1N0S1_fnd', 'F6T1N0S1_fnd', 'F6T1N0S1']
    # Models=['F6T1N0S1_fnd', 'F6T1N0S1']
    # Models=['F6T1N0S1_fnd']

    # Models=[ 'F2T0RNA_fnd' ,'F2T0N0S1_fnd' ,'F2T1RNA_fnd' ,'F3T1RNA_fnd' ,'F5T1RNA_fnd' ,'F2T1N0S1' ,'F0T2RNA' ,'F0T1RNA' ,'F0T2N0S1' ,'F6T0RNA' ,'F5T0N0S1_fnd' ,'F6T0RNA_fnd' ,'F6T0N0S1' ,'F6T1RNA' ,'F6T1RNA_fnd' ]

    #Models=['F2T0RNA'  , 'F2T0RNA_fnd', 'F2T0N0S1' , 'F2T1RNA'  , 'F2T1N0S1' , 'F0T2RNA'  , 'F0T2N0S1' , 'F6T0RNA' , 'F6T0RNA_fnd', 'F6T0N0S1' , 'F6T1RNA'  , 'F6T1N0S1' , 'F6T2N0S1' ]
    # Models=['F2T0RNA'  , 'F2T0RNA_fnd']
    # Models=['F6T1RNA']
    Models = ['F6T1N0S1']  #, 'F5T1N0S1_fnd']
    Models = ['F0T2N0S1']  #, 'F5T1N0S1_fnd']

    for model_name in Models:
        # if True:

        bSmallAngle = True

        opts = dict()
        opts[
            'rot_elastic_type'] = 'SmallRot'  #<<< Very important, 'SmallRot', or 'Body', will affect the rotation matrix
        # opts['rot_elastic_type']='Body' #<<< Very important, 'SmallRot', or 'Body', will affect the rotation matrix
        opts[
            'rot_elastic_smallAngle'] = False  #<<< Very important, will perform small angle approx: sin(nu q) = nu q and nu^2=0 !!! Will remove all nu^2 and nu^3 terms!! Not recommended, removes part of RNA "Y" inertia from mass matrix
        opts['orderMM'] = 1  #< order of taylor expansion for Mass Matrix
        opts['orderH'] = 1  #< order of taylor expansion for H term
        opts['fnd_loads'] = False
        opts['aero_torques'] = False
        opts['mergeFndTwr'] = model_name.find('_fnd') <= 0
        opts['yaw'] = 'zero'  # 'fixed', 'zero', or 'dynamic' if a DOF
        opts['tilt'] = 'fixed'  # 'fixed', 'zero', or 'dynamic' if a DOF
        opts['tiltShaft'] = True  # OpenFAST tilts shaft not nacelle
        #opts['linRot']       = False    # Linearize rotations matrices from the beginning
        opts[
            'linRot'] = True  # Linearize rotations matrices from the beginning
        opts['Mform'] = 'symbolic'  # or 'TaylorExpanded'
        opts['twrDOFDir'] = [
            'x', 'y', 'x', 'y'
        ]  # Order in which the flexible DOF of the tower are set

        # --- Esthetics Replacements for python
        replaceDict = {}
        replaceDict['theta_tilt'] = ('tilt', None)

        # --- Create model, solve equations and preform small angle approximation
        if model_name[0] == 'B':
            model = get_model_one_body(model_name, **opts)
        else:
            model = get_model(model_name, **opts)

        model.kaneEquations(Mform='TaylorExpanded')
        # ---
        extraSubs = model.shapeNormSubs
        # extraSubs+=[(Symbol('J_xx_T'),0)]
        # extraSubs+=[(Symbol('J_yy_T'),0)]
        # extraSubs+=[(Symbol('J_zz_T'),0)]
        # extraSubs+=[(Symbol('J_xx_N'),0)]
        # extraSubs+=[(Symbol('J_yy_N'),0)]
        # extraSubs+=[(Symbol('J_zz_N'),0)]
        # extraSubs+=[(diff(Symbol('phi'),time),0)]
        # extraSubs+=[(Symbol('M_N'),0)]
        #if model_name[0]=='B':
        #extraSubs+=[(Symbol('x_BG'),0)]
        #extraSubs+=[(Symbol('y_BG'),0)]

        print('Extra Subs:  ', extraSubs)

        # --- Linearization of non linear equations
        model.linearize(noAcc=True, noVel=False, extraSubs=extraSubs)

        # --- Small angle approximation and linearization
        if bSmallAngle:
            print('Small angles:', model.smallAngles)
            model.smallAngleApprox(model.smallAngles, extraSubs)
            model.smallAngleApproxEOM(model.smallAngles, extraSubs)
            model.smallAngleLinearize(noAcc=True,
                                      noVel=False,
                                      extraSubs=extraSubs)
            model.savePython(folder='_py',
                             variables=[
                                 'MM', 'FF', 'MMsa', 'FFsa', 'M', 'C', 'K',
                                 'B', 'Msa', 'Csa', 'Ksa', 'Bsa'
                             ],
                             replaceDict=replaceDict,
                             extraSubs=extraSubs)
        else:
            model.savePython(folder='_py',
                             variables=['MM', 'FF', 'M', 'C', 'K', 'B'],
                             replaceDict=replaceDict,
                             extraSubs=extraSubs,
                             doSimplify=False)

        # --- No vel
        print('>>>>>> NO VEL')
        velSubs = [(qd, 0)
                   for q, qd in zip(model.coordinates, model.coordinates_speed)
                   if q is not dynamicsymbols('psi')]
        velSubs += [(qd, 0) for q, qd in zip(model.coordinates, model.speeds)
                    if q is not dynamicsymbols('psi')]
        print('>>>', extraSubs)

        if bSmallAngle:
            model.savePython(folder='_py',
                             prefix='noVel_SA',
                             variables=['Msa', 'Csa', 'Ksa', 'Bsa'],
                             replaceDict=replaceDict,
                             extraSubs=extraSubs,
                             doSimplify=True,
                             velSubs=velSubs)
            model.saveTex(folder='_tex',
                          prefix='noVel_SA',
                          variables=['Msa', 'Csa', 'Ksa', 'Bsa'],
                          extraHeader='NoVelocity: ',
                          header=True,
                          doSimplify=True,
                          velSubs=velSubs)
Пример #34
0
def test_non_central_inertia():
    # This tests that the calculation of Fr* does not depend the point
    # about which the inertia of a rigid body is defined. This test solves
    # exercises 8.12, 8.17 from Kane 1985.

    # Declare symbols
    q1, q2, q3 = dynamicsymbols('q1:4')
    q1d, q2d, q3d = dynamicsymbols('q1:4', level=1)
    u1, u2, u3, u4, u5 = dynamicsymbols('u1:6')
    u_prime, R, M, g, e, f, theta = symbols('u\' R, M, g, e, f, theta')
    a, b, mA, mB, IA, J, K, t = symbols('a b mA mB IA J K t')
    Q1, Q2, Q3 = symbols('Q1, Q2 Q3')
    IA22, IA23, IA33 = symbols('IA22 IA23 IA33')

    # Reference Frames
    F = ReferenceFrame('F')
    P = F.orientnew('P', 'axis', [-theta, F.y])
    A = P.orientnew('A', 'axis', [q1, P.x])
    A.set_ang_vel(F, u1 * A.x + u3 * A.z)
    # define frames for wheels
    B = A.orientnew('B', 'axis', [q2, A.z])
    C = A.orientnew('C', 'axis', [q3, A.z])
    B.set_ang_vel(A, u4 * A.z)
    C.set_ang_vel(A, u5 * A.z)

    # define points D, S*, Q on frame A and their velocities
    pD = Point('D')
    pD.set_vel(A, 0)
    # u3 will not change v_D_F since wheels are still assumed to roll without slip.
    pD.set_vel(F, u2 * A.y)

    pS_star = pD.locatenew('S*', e * A.y)
    pQ = pD.locatenew('Q', f * A.y - R * A.x)
    for p in [pS_star, pQ]:
        p.v2pt_theory(pD, F, A)

    # masscenters of bodies A, B, C
    pA_star = pD.locatenew('A*', a * A.y)
    pB_star = pD.locatenew('B*', b * A.z)
    pC_star = pD.locatenew('C*', -b * A.z)
    for p in [pA_star, pB_star, pC_star]:
        p.v2pt_theory(pD, F, A)

    # points of B, C touching the plane P
    pB_hat = pB_star.locatenew('B^', -R * A.x)
    pC_hat = pC_star.locatenew('C^', -R * A.x)
    pB_hat.v2pt_theory(pB_star, F, B)
    pC_hat.v2pt_theory(pC_star, F, C)

    # the velocities of B^, C^ are zero since B, C are assumed to roll without slip
    kde = [q1d - u1, q2d - u4, q3d - u5]
    vc = [dot(p.vel(F), A.y) for p in [pB_hat, pC_hat]]

    # inertias of bodies A, B, C
    # IA22, IA23, IA33 are not specified in the problem statement, but are
    # necessary to define an inertia object. Although the values of
    # IA22, IA23, IA33 are not known in terms of the variables given in the
    # problem statement, they do not appear in the general inertia terms.
    inertia_A = inertia(A, IA, IA22, IA33, 0, IA23, 0)
    inertia_B = inertia(B, K, K, J)
    inertia_C = inertia(C, K, K, J)

    # define the rigid bodies A, B, C
    rbA = RigidBody('rbA', pA_star, A, mA, (inertia_A, pA_star))
    rbB = RigidBody('rbB', pB_star, B, mB, (inertia_B, pB_star))
    rbC = RigidBody('rbC', pC_star, C, mB, (inertia_C, pC_star))

    km = KanesMethod(F,
                     q_ind=[q1, q2, q3],
                     u_ind=[u1, u2],
                     kd_eqs=kde,
                     u_dependent=[u4, u5],
                     velocity_constraints=vc,
                     u_auxiliary=[u3])

    forces = [(pS_star, -M * g * F.x), (pQ, Q1 * A.x + Q2 * A.y + Q3 * A.z)]
    bodies = [rbA, rbB, rbC]
    with warns_deprecated_sympy():
        fr, fr_star = km.kanes_equations(forces, bodies)
    vc_map = solve(vc, [u4, u5])

    # KanesMethod returns the negative of Fr, Fr* as defined in Kane1985.
    fr_star_expected = Matrix([
        -(IA + 2 * J * b**2 / R**2 + 2 * K + mA * a**2 + 2 * mB * b**2) *
        u1.diff(t) - mA * a * u1 * u2,
        -(mA + 2 * mB + 2 * J / R**2) * u2.diff(t) + mA * a * u1**2, 0
    ])
    t = trigsimp(fr_star.subs(vc_map).subs({u3: 0})).doit().expand()
    assert ((fr_star_expected - t).expand() == zeros(3, 1))

    # define inertias of rigid bodies A, B, C about point D
    # I_S/O = I_S/S* + I_S*/O
    bodies2 = []
    for rb, I_star in zip([rbA, rbB, rbC], [inertia_A, inertia_B, inertia_C]):
        I = I_star + inertia_of_point_mass(rb.mass, rb.masscenter.pos_from(pD),
                                           rb.frame)
        bodies2.append(RigidBody('', rb.masscenter, rb.frame, rb.mass,
                                 (I, pD)))
    with warns_deprecated_sympy():
        fr2, fr_star2 = km.kanes_equations(forces, bodies2)

    t = trigsimp(fr_star2.subs(vc_map).subs({u3: 0})).doit()
    assert (fr_star_expected - t).expand() == zeros(3, 1)
Пример #35
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.testing.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([
Пример #36
0
def multi_mass_spring_damper(n=1,
                             apply_gravity=False,
                             apply_external_forces=False):
    r"""Returns a system containing the symbolic equations of motion and
    associated variables for a simple multi-degree of freedom point mass,
    spring, damper system with optional gravitational and external
    specified forces. For example, a two mass system under the influence of
    gravity and external forces looks like:

    ::

        ----------------
         |     |     |   | g
         \    | |    |   V
      k0 /    --- c0 |
         |     |     | x0, v0
        ---------    V
        |  m0   | -----
        ---------    |
         | |   |     |
         \ v  | |    |
      k1 / f0 --- c1 |
         |     |     | x1, v1
        ---------    V
        |  m1   | -----
        ---------
           | f1
           V

    Parameters
    ==========

    n : integer
        The number of masses in the serial chain.
    apply_gravity : boolean
        If true, gravity will be applied to each mass.
    apply_external_forces : boolean
        If true, a time varying external force will be applied to each mass.

    Returns
    =======

    kane : sympy.physics.mechanics.kane.KanesMethod
        A KanesMethod object.

    """

    mass = sm.symbols('m:{}'.format(n))
    stiffness = sm.symbols('k:{}'.format(n))
    damping = sm.symbols('c:{}'.format(n))

    acceleration_due_to_gravity = sm.symbols('g')

    coordinates = me.dynamicsymbols('x:{}'.format(n))
    speeds = me.dynamicsymbols('v:{}'.format(n))
    specifieds = me.dynamicsymbols('f:{}'.format(n))

    ceiling = me.ReferenceFrame('N')
    origin = me.Point('origin')
    origin.set_vel(ceiling, 0)

    points = [origin]
    kinematic_equations = []
    particles = []
    forces = []

    for i in range(n):

        center = points[-1].locatenew('center{}'.format(i),
                                      coordinates[i] * ceiling.x)
        center.set_vel(ceiling,
                       points[-1].vel(ceiling) + speeds[i] * ceiling.x)
        points.append(center)

        block = me.Particle('block{}'.format(i), center, mass[i])

        kinematic_equations.append(speeds[i] - coordinates[i].diff())

        total_force = (-stiffness[i] * coordinates[i] - damping[i] * speeds[i])
        try:
            total_force += (stiffness[i + 1] * coordinates[i + 1] +
                            damping[i + 1] * speeds[i + 1])
        except IndexError:  # no force from below on last mass
            pass

        if apply_gravity:
            total_force += mass[i] * acceleration_due_to_gravity

        if apply_external_forces:
            total_force += specifieds[i]

        forces.append((center, total_force * ceiling.x))

        particles.append(block)

    kane = me.KanesMethod(ceiling,
                          q_ind=coordinates,
                          u_ind=speeds,
                          kd_eqs=kinematic_equations)
    kane.kanes_equations(particles, forces)

    return kane
Пример #37
0
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 three
    # speed variables are need to describe this system, along with the disc's
    # mass and radius, and the local gravity (note that mass will drop out).
    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)
    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])
    w_R_N_qd = R.ang_vel_in(N)
    R.set_ang_vel(N, u1 * L.x + u2 * L.y + u3 * L.z)

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

    # This is a simple way to form the inertia dyadic.
    I = inertia(L, m / 4 * r**2, m / 2 * r**2, m / 4 * r**2)

    # Kinematic differential equations; how the generalized coordinate time
    # derivatives relate to generalized speeds.
    kd = [dot(R.ang_vel_in(N) - w_R_N_qd, uv) for uv in L]

    # Creation of the force list; it is the gravitational force at the mass
    # center of the disc. Then we create the disc by assigning a Point to the
    # center of mass attribute, a ReferenceFrame to the frame attribute, and mass
    # and inertia. Then we form the body list.
    ForceList = [(Dmc, -m * g * Y.z)]
    BodyD = RigidBody('BodyD', Dmc, R, m, (I, Dmc))
    BodyList = [BodyD]

    # Finally we form the equations of motion, using the same steps we did
    # before. Specify inertial frame, supply generalized speeds, supply
    # kinematic differential equation dictionary, compute Fr from the force
    # list and Fr* from the body list, compute the mass matrix and forcing
    # terms, then solve for the u dots (time derivatives of the generalized
    # speeds).
    KM = KanesMethod(N, q_ind=[q1, q2, q3], u_ind=[u1, u2, u3], kd_eqs=kd)
    with warns_deprecated_sympy():
        KM.kanes_equations(ForceList, BodyList)
    MM = KM.mass_matrix
    forcing = KM.forcing
    rhs = MM.inv() * forcing
    kdd = KM.kindiffdict()
    rhs = rhs.subs(kdd)
    rhs.simplify()
    assert rhs.expand() == Matrix([
        (6 * u2 * u3 * r - u3**2 * r * tan(q2) + 4 * g * sin(q2)) / (5 * r),
        -2 * u1 * u3 / 3, u1 * (-2 * u2 + u3 * tan(q2))
    ]).expand()
    assert simplify(KM.rhs() -
                    KM.mass_matrix_full.LUsolve(KM.forcing_full)) == zeros(
                        6, 1)

    # This code tests our output vs. benchmark values. When r=g=m=1, the
    # critical speed (where all eigenvalues of the linearized equations are 0)
    # is 1 / sqrt(3) for the upright case.
    A = KM.linearize(A_and_B=True)[0]
    A_upright = A.subs({
        r: 1,
        g: 1,
        m: 1
    }).subs({
        q1: 0,
        q2: 0,
        q3: 0,
        u1: 0,
        u3: 0
    })
    import sympy
    assert sympy.sympify(A_upright.subs({u2: 1 / sqrt(3)})).eigenvals() == {
        S.Zero: 6
    }
Пример #38
0
def n_link_pendulum_on_cart(n=1, cart_force=True, joint_torques=False):
    r"""Returns the system containing the symbolic first order equations of
    motion for a 2D n-link pendulum on a sliding cart under the influence of
    gravity.

    ::

                  |
         o    y   v
          \ 0 ^   g
           \  |
          --\-|----
          |  \|   |
      F-> |   o --|---> x
          |       |
          ---------
           o     o

    Parameters
    ==========

    n : integer
        The number of links in the pendulum.
    cart_force : boolean, default=True
        If true an external specified lateral force is applied to the cart.
    joint_torques : boolean, default=False
        If true joint torques will be added as specified inputs at each
        joint.

    Returns
    =======

    kane : sympy.physics.mechanics.kane.KanesMethod
        A KanesMethod object.

    Notes
    =====

    The degrees of freedom of the system are n + 1, i.e. one for each
    pendulum link and one for the lateral motion of the cart.

    M x' = F, where x = [u0, ..., un+1, q0, ..., qn+1]

    The joint angles are all defined relative to the ground where the x axis
    defines the ground line and the y axis points up. The joint torques are
    applied between each adjacent link and the between the cart and the
    lower link where a positive torque corresponds to positive angle.

    """
    if n <= 0:
        raise ValueError('The number of links must be a positive integer.')

    q = me.dynamicsymbols('q:{}'.format(n + 1))
    u = me.dynamicsymbols('u:{}'.format(n + 1))

    if joint_torques is True:
        T = me.dynamicsymbols('T1:{}'.format(n + 1))

    m = sm.symbols('m:{}'.format(n + 1))
    l = sm.symbols('l:{}'.format(n))
    g, t = sm.symbols('g t')

    I = me.ReferenceFrame('I')
    O = me.Point('O')
    O.set_vel(I, 0)

    P0 = me.Point('P0')
    P0.set_pos(O, q[0] * I.x)
    P0.set_vel(I, u[0] * I.x)
    Pa0 = me.Particle('Pa0', P0, m[0])

    frames = [I]
    points = [P0]
    particles = [Pa0]
    forces = [(P0, -m[0] * g * I.y)]
    kindiffs = [q[0].diff(t) - u[0]]

    if cart_force is True or joint_torques is True:
        specified = []
    else:
        specified = None

    for i in range(n):
        Bi = I.orientnew('B{}'.format(i), 'Axis', [q[i + 1], I.z])
        Bi.set_ang_vel(I, u[i + 1] * I.z)
        frames.append(Bi)

        Pi = points[-1].locatenew('P{}'.format(i + 1), l[i] * Bi.y)
        Pi.v2pt_theory(points[-1], I, Bi)
        points.append(Pi)

        Pai = me.Particle('Pa' + str(i + 1), Pi, m[i + 1])
        particles.append(Pai)

        forces.append((Pi, -m[i + 1] * g * I.y))

        if joint_torques is True:

            specified.append(T[i])

            if i == 0:
                forces.append((I, -T[i] * I.z))

            if i == n - 1:
                forces.append((Bi, T[i] * I.z))
            else:
                forces.append((Bi, T[i] * I.z - T[i + 1] * I.z))

        kindiffs.append(q[i + 1].diff(t) - u[i + 1])

    if cart_force is True:
        F = me.dynamicsymbols('F')
        forces.append((P0, F * I.x))
        specified.append(F)

    kane = me.KanesMethod(I, q_ind=q, u_ind=u, kd_eqs=kindiffs)
    kane.kanes_equations(particles, forces)

    return kane
Пример #39
0
def THETA(qtd):
    theta = [0]
    for i in range(1,qtd+1):
        theta.append(dynamicsymbols('theta'+str(i)))
    return theta
Пример #40
0
#!/usr/bin/env python
# -*- coding: utf-8 -*-
"""Exercise 10.3 from Kane 1985."""

from __future__ import division
from sympy import collect, expand, sin, cos, pi, radsimp, solve, sqrt, symbols
from sympy.physics.mechanics import ReferenceFrame, RigidBody, Point
from sympy.physics.mechanics import dot, dynamicsymbols, inertia, msprint

q1, q2, q3, q4, q5, q6, q7 = q = dynamicsymbols('q1:8')
u1, u2, u3, u4, u5, u6, u7 = u = dynamicsymbols('q1:8', level=1)

M, J, I11, I22, m, r, b = symbols('M J I11 I22 m r b',
                                  real=True,
                                  positive=True)
omega, t = symbols('ω t')

theta = 30 * pi / 180  # radians
b = r * (1 + sin(theta)) / (cos(theta) - sin(theta))
# Note: using b as found in Ex3.10. Pure rolling between spheres and race R
# is likely a typo and should be between spheres and cone C.

# define reference frames
R = ReferenceFrame('R')  # fixed race rf, let R.z point upwards
A = R.orientnew('A', 'axis', [q7, R.z])  # rf that rotates with S* about R.z
# B.x, B.z are parallel with face of cone, B.y is perpendicular
B = A.orientnew('B', 'axis', [-theta, A.x])
S = ReferenceFrame('S')
S.set_ang_vel(A, u1 * A.x + u2 * A.y + u3 * A.z)
C = ReferenceFrame('C')
C.set_ang_vel(A, u4 * B.x + u5 * B.y + u6 * B.z)
Пример #41
0
def test_sub_qdot():
    # This test solves exercises 8.12, 8.17 from Kane 1985 and defines
    # some velocities in terms of q, qdot.

    ## --- Declare symbols ---
    q1, q2, q3 = dynamicsymbols('q1:4')
    q1d, q2d, q3d = dynamicsymbols('q1:4', level=1)
    u1, u2, u3 = dynamicsymbols('u1:4')
    u_prime, R, M, g, e, f, theta = symbols('u\' R, M, g, e, f, theta')
    a, b, mA, mB, IA, J, K, t = symbols('a b mA mB IA J K t')
    IA22, IA23, IA33 = symbols('IA22 IA23 IA33')
    Q1, Q2, Q3 = symbols('Q1 Q2 Q3')

    # --- Reference Frames ---
    F = ReferenceFrame('F')
    P = F.orientnew('P', 'axis', [-theta, F.y])
    A = P.orientnew('A', 'axis', [q1, P.x])
    A.set_ang_vel(F, u1 * A.x + u3 * A.z)
    # define frames for wheels
    B = A.orientnew('B', 'axis', [q2, A.z])
    C = A.orientnew('C', 'axis', [q3, A.z])

    ## --- define points D, S*, Q on frame A and their velocities ---
    pD = Point('D')
    pD.set_vel(A, 0)
    # u3 will not change v_D_F since wheels are still assumed to roll w/o slip
    pD.set_vel(F, u2 * A.y)

    pS_star = pD.locatenew('S*', e * A.y)
    pQ = pD.locatenew('Q', f * A.y - R * A.x)
    # masscenters of bodies A, B, C
    pA_star = pD.locatenew('A*', a * A.y)
    pB_star = pD.locatenew('B*', b * A.z)
    pC_star = pD.locatenew('C*', -b * A.z)
    for p in [pS_star, pQ, pA_star, pB_star, pC_star]:
        p.v2pt_theory(pD, F, A)

    # points of B, C touching the plane P
    pB_hat = pB_star.locatenew('B^', -R * A.x)
    pC_hat = pC_star.locatenew('C^', -R * A.x)
    pB_hat.v2pt_theory(pB_star, F, B)
    pC_hat.v2pt_theory(pC_star, F, C)

    # --- relate qdot, u ---
    # the velocities of B^, C^ are zero since B, C are assumed to roll w/o slip
    kde = [dot(p.vel(F), A.y) for p in [pB_hat, pC_hat]]
    kde += [u1 - q1d]
    kde_map = solve(kde, [q1d, q2d, q3d])
    for k, v in list(kde_map.items()):
        kde_map[k.diff(t)] = v.diff(t)

    # inertias of bodies A, B, C
    # IA22, IA23, IA33 are not specified in the problem statement, but are
    # necessary to define an inertia object. Although the values of
    # IA22, IA23, IA33 are not known in terms of the variables given in the
    # problem statement, they do not appear in the general inertia terms.
    inertia_A = inertia(A, IA, IA22, IA33, 0, IA23, 0)
    inertia_B = inertia(B, K, K, J)
    inertia_C = inertia(C, K, K, J)

    # define the rigid bodies A, B, C
    rbA = RigidBody('rbA', pA_star, A, mA, (inertia_A, pA_star))
    rbB = RigidBody('rbB', pB_star, B, mB, (inertia_B, pB_star))
    rbC = RigidBody('rbC', pC_star, C, mB, (inertia_C, pC_star))

    ## --- use kanes method ---
    km = KanesMethod(F, [q1, q2, q3], [u1, u2], kd_eqs=kde, u_auxiliary=[u3])

    forces = [(pS_star, -M * g * F.x), (pQ, Q1 * A.x + Q2 * A.y + Q3 * A.z)]
    bodies = [rbA, rbB, rbC]

    # Q2 = -u_prime * u2 * Q1 / sqrt(u2**2 + f**2 * u1**2)
    # -u_prime * R * u2 / sqrt(u2**2 + f**2 * u1**2) = R / Q1 * Q2
    fr_expected = Matrix([
        f * Q3 + M * g * e * sin(theta) * cos(q1),
        Q2 + M * g * sin(theta) * sin(q1),
        e * M * g * cos(theta) - Q1 * f - Q2 * R
    ])
    #Q1 * (f - u_prime * R * u2 / sqrt(u2**2 + f**2 * u1**2)))])
    fr_star_expected = Matrix([
        -(IA + 2 * J * b**2 / R**2 + 2 * K + mA * a**2 + 2 * mB * b**2) *
        u1.diff(t) - mA * a * u1 * u2,
        -(mA + 2 * mB + 2 * J / R**2) * u2.diff(t) + mA * a * u1**2, 0
    ])

    with warns_deprecated_sympy():
        fr, fr_star = km.kanes_equations(forces, bodies)
    assert (fr.expand() == fr_expected.expand())
    assert ((fr_star_expected - trigsimp(fr_star)).expand() == zeros(3, 1))
Пример #42
0
def test_linearize_rolling_disc_kane():
    # Symbols for time and constant parameters
    t, r, m, g, v = symbols('t r m g v')

    # Configuration variables and their time derivatives
    q1, q2, q3, q4, q5, q6 = q = dynamicsymbols('q1:7')
    q1d, q2d, q3d, q4d, q5d, q6d = qd = [qi.diff(t) for qi in q]

    # Generalized speeds and their time derivatives
    u = dynamicsymbols('u:6')
    u1, u2, u3, u4, u5, u6 = u = dynamicsymbols('u1:7')
    u1d, u2d, u3d, u4d, u5d, u6d = [ui.diff(t) for ui in u]

    # Reference frames
    N = ReferenceFrame('N')  # Inertial frame
    NO = Point('NO')  # Inertial origin
    A = N.orientnew('A', 'Axis', [q1, N.z])  # Yaw intermediate frame
    B = A.orientnew('B', 'Axis', [q2, A.x])  # Lean intermediate frame
    C = B.orientnew('C', 'Axis', [q3, B.y])  # Disc fixed frame
    CO = NO.locatenew('CO', q4 * N.x + q5 * N.y + q6 * N.z)  # Disc center

    # Disc angular velocity in N expressed using time derivatives of coordinates
    w_c_n_qd = C.ang_vel_in(N)
    w_b_n_qd = B.ang_vel_in(N)

    # Inertial angular velocity and angular acceleration of disc fixed frame
    C.set_ang_vel(N, u1 * B.x + u2 * B.y + u3 * B.z)

    # Disc center velocity in N expressed using time derivatives of coordinates
    v_co_n_qd = CO.pos_from(NO).dt(N)

    # Disc center velocity in N expressed using generalized speeds
    CO.set_vel(N, u4 * C.x + u5 * C.y + u6 * C.z)

    # Disc Ground Contact Point
    P = CO.locatenew('P', r * B.z)
    P.v2pt_theory(CO, N, C)

    # Configuration constraint
    f_c = Matrix([q6 - dot(CO.pos_from(P), N.z)])

    # Velocity level constraints
    f_v = Matrix([dot(P.vel(N), uv) for uv in C])

    # Kinematic differential equations
    kindiffs = Matrix([dot(w_c_n_qd - C.ang_vel_in(N), uv) for uv in B] +
                      [dot(v_co_n_qd - CO.vel(N), uv) for uv in N])
    qdots = solve(kindiffs, qd)

    # Set angular velocity of remaining frames
    B.set_ang_vel(N, w_b_n_qd.subs(qdots))
    C.set_ang_acc(
        N,
        C.ang_vel_in(N).dt(B) + cross(B.ang_vel_in(N), C.ang_vel_in(N)))

    # Active forces
    F_CO = m * g * A.z

    # Create inertia dyadic of disc C about point CO
    I = (m * r**2) / 4
    J = (m * r**2) / 2
    I_C_CO = inertia(C, I, J, I)

    Disc = RigidBody('Disc', CO, C, m, (I_C_CO, CO))
    BL = [Disc]
    FL = [(CO, F_CO)]
    KM = KanesMethod(N, [q1, q2, q3, q4, q5], [u1, u2, u3],
                     kd_eqs=kindiffs,
                     q_dependent=[q6],
                     configuration_constraints=f_c,
                     u_dependent=[u4, u5, u6],
                     velocity_constraints=f_v)
    (fr, fr_star) = KM.kanes_equations(FL, BL)

    # Test generalized form equations
    linearizer = KM.to_linearizer()
    assert linearizer.f_c == f_c
    assert linearizer.f_v == f_v
    assert linearizer.f_a == f_v.diff(t)
    sol = solve(linearizer.f_0 + linearizer.f_1, qd)
    for qi in qd:
        assert sol[qi] == qdots[qi]
    assert simplify(linearizer.f_2 + linearizer.f_3 - fr - fr_star) == Matrix(
        [0, 0, 0])

    # Perform the linearization
    # Precomputed operating point
    q_op = {q6: -r * cos(q2)}
    u_op = {
        u1: 0,
        u2: sin(q2) * q1d + q3d,
        u3: cos(q2) * q1d,
        u4: -r * (sin(q2) * q1d + q3d) * cos(q3),
        u5: 0,
        u6: -r * (sin(q2) * q1d + q3d) * sin(q3)
    }
    qd_op = {
        q2d: 0,
        q4d: -r * (sin(q2) * q1d + q3d) * cos(q1),
        q5d: -r * (sin(q2) * q1d + q3d) * sin(q1),
        q6d: 0
    }
    ud_op = {
        u1d:
        4 * g * sin(q2) / (5 * r) + sin(2 * q2) * q1d**2 / 2 +
        6 * cos(q2) * q1d * q3d / 5,
        u2d:
        0,
        u3d:
        0,
        u4d:
        r * (sin(q2) * sin(q3) * q1d * q3d + sin(q3) * q3d**2),
        u5d:
        r * (4 * g * sin(q2) /
             (5 * r) + sin(2 * q2) * q1d**2 / 2 + 6 * cos(q2) * q1d * q3d / 5),
        u6d:
        -r * (sin(q2) * cos(q3) * q1d * q3d + cos(q3) * q3d**2)
    }

    A, B = linearizer.linearize(op_point=[q_op, u_op, qd_op, ud_op],
                                A_and_B=True,
                                simplify=True)

    upright_nominal = {q1d: 0, q2: 0, m: 1, r: 1, g: 1}

    # Precomputed solution
    A_sol = Matrix([[0, 0, 0, 0, 0, 0, 0, 1], [0, 0, 0, 0, 0, 1, 0, 0],
                    [0, 0, 0, 0, 0, 0, 1, 0],
                    [sin(q1) * q3d, 0, 0, 0, 0, -sin(q1), -cos(q1), 0],
                    [-cos(q1) * q3d, 0, 0, 0, 0,
                     cos(q1), -sin(q1), 0],
                    [0, 4 / 5, 0, 0, 0, 0, 0, 6 * q3d / 5],
                    [0, 0, 0, 0, 0, 0, 0, 0], [0, 0, 0, 0, 0, -2 * q3d, 0, 0]])
    B_sol = Matrix([])

    # Check that linearization is correct
    assert A.subs(upright_nominal) == A_sol
    assert B.subs(upright_nominal) == B_sol

    # Check eigenvalues at critical speed are all zero:
    assert A.subs(upright_nominal).subs(q3d, 1 / sqrt(3)).eigenvals() == {0: 8}
#this file is used to generate a serialized function that can be used to estimate
# next states given current states
#ASSUMES GRAVITY SUFFICIENTLY COMPENSATED BY ROBOT

init_vprinting(use_latex='mathjax', pretty_print=True)

#Kinematics ------------------------------
#init reference frames, assume base attached to floor
inertial_frame = ReferenceFrame('I')
j0_frame = ReferenceFrame('J0')
j1_frame = ReferenceFrame('J1')
j2_frame = ReferenceFrame('J2')

#declare dynamic symbols for the three joints
theta0, theta1, theta2 = dynamicsymbols('theta0, theta1, theta2')

#orient frames
j0_frame.orient(inertial_frame, 'Axis', (theta0, inertial_frame.y))
j1_frame.orient(j0_frame, 'Axis', (theta1, j0_frame.z))
j2_frame.orient(j1_frame, 'Axis', (theta2, j1_frame.z))

#TODO figure out better name for joint points
#init joints
joint0 = Point('j0')
j0_length = symbols('l_j0')
joint1 = Point('j1')
joint1.set_pos(joint0, j0_length * j0_frame.y)
j1_length = symbols('l_j1')
joint2 = Point('j2')
joint2.set_pos(joint1, j1_length * j1_frame.y)
Пример #44
0
# %%
import sympy as sp
from sympy.physics.mechanics import (dynamicsymbols, ReferenceFrame, Point,
                                     Particle, KanesMethod)
# %%
n = 2
q = dynamicsymbols('q:' + str(n + 1))  # Generalized coordinates
u = dynamicsymbols('u:' + str(n + 1))  # Generalized speeds
f = dynamicsymbols('f')  # Force applied to the cart

m = sp.symbols('m:' + str(n + 1))  # Mass of each bob

# %%
import intersection from intersection1
import selectphi from selecting_phi
#jointlimits
#theta_1_u=
#theta_1_l=
#theta_2_u=
#theta_2_l=
#theta_3_u=
#theta_3_l=
#theta_4_u=
#theta_4_l=
#theta_5_u=
#theta_5_l=
#theta_6_u=
#theta_6_l=
alpha,beta,gamma,p_x,p_y,p_z,l_bs,l_se,l_we,l_wt,theta_1,theta_2,theta_3,theta_4,theta_5,theta_6,u_x,u_y,u_z,theta_11,theta_22,theta_33,theta_44,alpha_1,beta_2,phi=dynamicsymbols('alpha,beta,gamma,p_x,p_y,p_z,l_bs,l_se,l_we,l_wt,theta_1,theta_2,theta_3,theta_4,theta_5,theta_6,u_x,u_y,u_z,theta_11,theta_22,theta_33,theta_44,alpha_1,beta_2,phi')
rot_end=sp.Matrix([[sp.cos(beta), -sp.sin(beta)*sp.cos(gamma), sp.sin(beta)*sp.sin(gamma)],
                 [sp.sin(beta)*sp.cos(alpha), (sp.cos(alpha)*sp.cos(beta)*sp.cos(gamma)-sp.sin(alpha)*sp.sin(gamma)), (-sp.cos(alpha)*sp.cos(beta)*sp.sin(gamma)-sp.sin(alpha)*sp.cos(gamma))],
                 [sp.sin(alpha)*sp.sin(beta), (sp.cos(alpha)*sp.sin(gamma)+ sp.cos(beta)*sp.cos(gamma)*sin(alpha)), (sp.cos(alpha)*sp.cos(gamma)-sp.cos(beta)*sp.sin(alpha)*sin(gamma))]])
#Euler angle XZX for orientation#
#print(rot_end[0,2])
pos_end=sp.Matrix([[p_x],[p_y],[p_z]])
#print(pos_end.shape)
w=np.zeros((3,1))
l_wt=sp.Matrix([[0],[0],[l_wt]])

w=pos_end-(rot_end*(l_wt))
w_x=w[0,0]
w_y=w[1,0]
w_z=w[2,0]
L_bs=sp.Matrix([[0],[0],[l_bs]])
Пример #46
0
def L(qtd):
    l = [0]
    for i in range(1,qtd+1):
        l.append(dynamicsymbols('l'+str(i)))
    return l
Пример #47
0
#!/usr/bin/env python
# -*- coding: utf-8 -*-
"""Exercise 10.5 from Kane 1985.
Answer does not match text.
"""

from __future__ import division
from sympy import expand, symbols, trigsimp, sin, cos, S
from sympy.physics.mechanics import ReferenceFrame, RigidBody, Point
from sympy.physics.mechanics import dynamicsymbols, inertia, msprint

m, a, b = symbols('m a b')
q1, q2 = dynamicsymbols('q1, q2')
q1d, q2d = dynamicsymbols('q1, q2', level=1)

# reference frames
# N.x parallel to horizontal line, N.y parallel to line AC
N = ReferenceFrame('N')
A = N.orientnew('A', 'axis', [-q1, N.y])
B = A.orientnew('B', 'axis', [-q2, A.x])

# points B*, O
pO = Point('O')
pB_star = pO.locatenew('B*', S(1) / 3 * (2 * a * B.x - b * B.y))
pO.set_vel(N, 0)
pB_star.v2pt_theory(pO, N, B)

# rigidbody B
I_B_Bs = inertia(B, m * b**2 / 18, m * a**2 / 18, m * (a**2 + b**2) / 18)
rbB = RigidBody('rbB', pB_star, B, m, (I_B_Bs, pB_star))
Пример #48
0
#!/usr/bin/env python

from sympy import symbols
import sympy.physics.mechanics as me

print("Defining the problem.")

# The conical pendulum will have three links and three bobs.
n = 3

# Each link's orientation is described by two spaced fixed angles: alpha and
# beta.

# Generalized coordinates
alpha = me.dynamicsymbols('alpha:{}'.format(n))
beta = me.dynamicsymbols('beta:{}'.format(n))

# Generalized speeds
omega = me.dynamicsymbols('omega:{}'.format(n))
delta = me.dynamicsymbols('delta:{}'.format(n))

# At each joint there are point masses (i.e. the bobs).
m_bob = symbols('m:{}'.format(n))

# Each link is modeled as a cylinder so it will have a length, mass, and a
# symmetric inertia tensor.
l = symbols('l:{}'.format(n))
m_link = symbols('M:{}'.format(n))
Ixx = symbols('Ixx:{}'.format(n))
Iyy = symbols('Iyy:{}'.format(n))
Izz = symbols('Izz:{}'.format(n))
Пример #49
0
def test_aux_dep():
    # This test is about rolling disc dynamics, comparing the results found
    # with KanesMethod to those found when deriving the equations "manually"
    # with SymPy.
    # The terms Fr, Fr*, and Fr*_steady are all compared between the two
    # methods. Here, Fr*_steady refers to the generalized inertia forces for an
    # equilibrium configuration.
    # Note: comparing to the test of test_rolling_disc() in test_kane.py, this
    # test also tests auxiliary speeds and configuration and motion constraints
    #, seen in  the generalized dependent coordinates q[3], and depend speeds
    # u[3], u[4] and u[5].


    # First, mannual derivation of Fr, Fr_star, Fr_star_steady.

    # Symbols for time and constant parameters.
    # Symbols for contact forces: Fx, Fy, Fz.
    t, r, m, g, I, J = symbols('t r m g I J')
    Fx, Fy, Fz = symbols('Fx Fy Fz')

    # Configuration variables and their time derivatives:
    # q[0] -- yaw
    # q[1] -- lean
    # q[2] -- spin
    # q[3] -- dot(-r*B.z, A.z) -- distance from ground plane to disc center in
    #         A.z direction
    # Generalized speeds and their time derivatives:
    # u[0] -- disc angular velocity component, disc fixed x direction
    # u[1] -- disc angular velocity component, disc fixed y direction
    # u[2] -- disc angular velocity component, disc fixed z direction
    # u[3] -- disc velocity component, A.x direction
    # u[4] -- disc velocity component, A.y direction
    # u[5] -- disc velocity component, A.z direction
    # Auxiliary generalized speeds:
    # ua[0] -- contact point auxiliary generalized speed, A.x direction
    # ua[1] -- contact point auxiliary generalized speed, A.y direction
    # ua[2] -- contact point auxiliary generalized speed, A.z direction
    q = dynamicsymbols('q:4')
    qd = [qi.diff(t) for qi in q]
    u = dynamicsymbols('u:6')
    ud = [ui.diff(t) for ui in u]
    ud_zero = dict(zip(ud, [0.]*len(ud)))
    ua = dynamicsymbols('ua:3')
    ua_zero = dict(zip(ua, [0.]*len(ua)))

    # Reference frames:
    # Yaw intermediate frame: A.
    # Lean intermediate frame: B.
    # Disc fixed frame: C.
    N = ReferenceFrame('N')
    A = N.orientnew('A', 'Axis', [q[0], N.z])
    B = A.orientnew('B', 'Axis', [q[1], A.x])
    C = B.orientnew('C', 'Axis', [q[2], B.y])

    # Angular velocity and angular acceleration of disc fixed frame
    # u[0], u[1] and u[2] are generalized independent speeds.
    C.set_ang_vel(N, u[0]*B.x + u[1]*B.y + u[2]*B.z)
    C.set_ang_acc(N, C.ang_vel_in(N).diff(t, B)
                   + cross(B.ang_vel_in(N), C.ang_vel_in(N)))

    # Velocity and acceleration of points:
    # Disc-ground contact point: P.
    # Center of disc: O, defined from point P with depend coordinate: q[3]
    # u[3], u[4] and u[5] are generalized dependent speeds.
    P = Point('P')
    P.set_vel(N, ua[0]*A.x + ua[1]*A.y + ua[2]*A.z)
    O = P.locatenew('O', q[3]*A.z + r*sin(q[1])*A.y)
    O.set_vel(N, u[3]*A.x + u[4]*A.y + u[5]*A.z)
    O.set_acc(N, O.vel(N).diff(t, A) + cross(A.ang_vel_in(N), O.vel(N)))

    # Kinematic differential equations:
    # Two equalities: one is w_c_n_qd = C.ang_vel_in(N) in three coordinates
    #                 directions of B, for qd0, qd1 and qd2.
    #                 the other is v_o_n_qd = O.vel(N) in A.z direction for qd3.
    # Then, solve for dq/dt's in terms of u's: qd_kd.
    w_c_n_qd = qd[0]*A.z + qd[1]*B.x + qd[2]*B.y
    v_o_n_qd = O.pos_from(P).diff(t, A) + cross(A.ang_vel_in(N), O.pos_from(P))
    kindiffs = Matrix([dot(w_c_n_qd - C.ang_vel_in(N), uv) for uv in B] +
                      [dot(v_o_n_qd - O.vel(N), A.z)])
    qd_kd = solve(kindiffs, qd)

    # Values of generalized speeds during a steady turn for later substitution
    # into the Fr_star_steady.
    steady_conditions = solve(kindiffs.subs({qd[1] : 0, qd[3] : 0}), u)
    steady_conditions.update({qd[1] : 0, qd[3] : 0})

    # Partial angular velocities and velocities.
    partial_w_C = [C.ang_vel_in(N).diff(ui, N) for ui in u + ua]
    partial_v_O = [O.vel(N).diff(ui, N) for ui in u + ua]
    partial_v_P = [P.vel(N).diff(ui, N) for ui in u + ua]

    # Configuration constraint: f_c, the projection of radius r in A.z direction
    #                                is q[3].
    # Velocity constraints: f_v, for u3, u4 and u5.
    # Acceleration constraints: f_a.
    f_c = Matrix([dot(-r*B.z, A.z) - q[3]])
    f_v = Matrix([dot(O.vel(N) - (P.vel(N) + cross(C.ang_vel_in(N),
        O.pos_from(P))), ai).expand() for ai in A])
    v_o_n = cross(C.ang_vel_in(N), O.pos_from(P))
    a_o_n = v_o_n.diff(t, A) + cross(A.ang_vel_in(N), v_o_n)
    f_a = Matrix([dot(O.acc(N) - a_o_n, ai) for ai in A])

    # Solve for constraint equations in the form of
    #                           u_dependent = A_rs * [u_i; u_aux].
    # First, obtain constraint coefficient matrix:  M_v * [u; ua] = 0;
    # Second, taking u[0], u[1], u[2] as independent,
    #         taking u[3], u[4], u[5] as dependent,
    #         rearranging the matrix of M_v to be A_rs for u_dependent.
    # Third, u_aux ==0 for u_dep, and resulting dictionary of u_dep_dict.
    M_v = zeros(3, 9)
    for i in range(3):
        for j, ui in enumerate(u + ua):
            M_v[i, j] = f_v[i].diff(ui)

    M_v_i = M_v[:, :3]
    M_v_d = M_v[:, 3:6]
    M_v_aux = M_v[:, 6:]
    M_v_i_aux = M_v_i.row_join(M_v_aux)
    A_rs = - M_v_d.inv() * M_v_i_aux

    u_dep = A_rs[:, :3] * Matrix(u[:3])
    u_dep_dict = dict(zip(u[3:], u_dep))

    # Active forces: F_O acting on point O; F_P acting on point P.
    # Generalized active forces (unconstrained): Fr_u = F_point * pv_point.
    F_O = m*g*A.z
    F_P = Fx * A.x + Fy * A.y + Fz * A.z
    Fr_u = Matrix([dot(F_O, pv_o) + dot(F_P, pv_p) for pv_o, pv_p in
            zip(partial_v_O, partial_v_P)])

    # Inertia force: R_star_O.
    # Inertia of disc: I_C_O, where J is a inertia component about principal axis.
    # Inertia torque: T_star_C.
    # Generalized inertia forces (unconstrained): Fr_star_u.
    R_star_O = -m*O.acc(N)
    I_C_O = inertia(B, I, J, I)
    T_star_C = -(dot(I_C_O, C.ang_acc_in(N)) \
                 + cross(C.ang_vel_in(N), dot(I_C_O, C.ang_vel_in(N))))
    Fr_star_u = Matrix([dot(R_star_O, pv) + dot(T_star_C, pav) for pv, pav in
                        zip(partial_v_O, partial_w_C)])

    # Form nonholonomic Fr: Fr_c, and nonholonomic Fr_star: Fr_star_c.
    # Also, nonholonomic Fr_star in steady turning condition: Fr_star_steady.
    Fr_c = Fr_u[:3, :].col_join(Fr_u[6:, :]) + A_rs.T * Fr_u[3:6, :]
    Fr_star_c = Fr_star_u[:3, :].col_join(Fr_star_u[6:, :])\
                + A_rs.T * Fr_star_u[3:6, :]
    Fr_star_steady = Fr_star_c.subs(ud_zero).subs(u_dep_dict)\
            .subs(steady_conditions).subs({q[3]: -r*cos(q[1])}).expand()


    # Second, using KaneMethod in mechanics for fr, frstar and frstar_steady.

    # Rigid Bodies: disc, with inertia I_C_O.
    iner_tuple = (I_C_O, O)
    disc = RigidBody('disc', O, C, m, iner_tuple)
    bodyList = [disc]

    # Generalized forces: Gravity: F_o; Auxiliary forces: F_p.
    F_o = (O, F_O)
    F_p = (P, F_P)
    forceList = [F_o,  F_p]

    # KanesMethod.
    kane = KanesMethod(
        N, q_ind= q[:3], u_ind= u[:3], kd_eqs=kindiffs,
        q_dependent=q[3:], configuration_constraints = f_c,
        u_dependent=u[3:], velocity_constraints= f_v,
        u_auxiliary=ua
        )

    # fr, frstar, frstar_steady and kdd(kinematic differential equations).
    with warnings.catch_warnings():
        warnings.filterwarnings("ignore", category=SymPyDeprecationWarning)
        (fr, frstar)= kane.kanes_equations(forceList, bodyList)
    frstar_steady = frstar.subs(ud_zero).subs(u_dep_dict).subs(steady_conditions)\
                    .subs({q[3]: -r*cos(q[1])}).expand()
    kdd = kane.kindiffdict()

    assert Matrix(Fr_c).expand() == fr.expand()
    assert Matrix(Fr_star_c.subs(kdd)).expand() == frstar.expand()
    assert (simplify(Matrix(Fr_star_steady).expand()) ==
            simplify(frstar_steady.expand()))
Пример #50
0
def test_pinjoint_arbitrary_axis():
    theta, omega = dynamicsymbols('theta_J, omega_J')

    # When the bodies are attached though masscenters but axess are opposite.
    N, A, P, C = _generate_body()
    PinJoint('J', P, C, child_axis=-A.x)

    assert -A.x.angle_between(N.x) == -pi
    assert -A.x.express(N) == N.x
    assert A.dcm(N) == Matrix([[-1, 0, 0], [0, -cos(theta), -sin(theta)],
                               [0, -sin(theta), cos(theta)]])
    assert A.ang_vel_in(N) == omega * N.x
    assert A.ang_vel_in(N).magnitude() == sqrt(omega**2)
    assert C.masscenter.pos_from(P.masscenter) == 0
    assert C.masscenter.pos_from(P.masscenter).express(N).simplify() == 0
    assert C.masscenter.vel(N) == 0

    # When axes are different and parent joint is at masscenter but child joint
    # is at a unit vector from child masscenter.
    N, A, P, C = _generate_body()
    PinJoint('J', P, C, child_axis=A.y, child_joint_pos=A.x)

    assert A.y.angle_between(N.x) == 0  # Axis are aligned
    assert A.y.express(N) == N.x
    assert A.dcm(N) == Matrix([[0, -cos(theta), -sin(theta)], [1, 0, 0],
                               [0, -sin(theta), cos(theta)]])
    assert A.ang_vel_in(N) == omega * N.x
    assert A.ang_vel_in(N).express(A) == omega * A.y
    assert A.ang_vel_in(N).magnitude() == sqrt(omega**2)
    angle = A.ang_vel_in(N).angle_between(A.y)
    assert expand_mul(angle).xreplace({omega: 1}) == 0
    assert C.masscenter.vel(N) == omega * A.z
    assert C.masscenter.pos_from(P.masscenter) == -A.x
    assert (C.masscenter.pos_from(
        P.masscenter).express(N).simplify() == cos(theta) * N.y +
            sin(theta) * N.z)
    assert C.masscenter.vel(N).angle_between(A.x) == pi / 2

    # Similar to previous case but wrt parent body
    N, A, P, C = _generate_body()
    PinJoint('J', P, C, parent_axis=N.y, parent_joint_pos=N.x)

    assert N.y.angle_between(A.x) == 0  # Axis are aligned
    assert N.y.express(A) == A.x
    assert A.dcm(N) == Matrix([[0, 1, 0], [-cos(theta), 0,
                                           sin(theta)],
                               [sin(theta), 0, cos(theta)]])
    assert A.ang_vel_in(N) == omega * N.y
    assert A.ang_vel_in(N).express(A) == omega * A.x
    assert A.ang_vel_in(N).magnitude() == sqrt(omega**2)
    angle = A.ang_vel_in(N).angle_between(A.x)
    assert expand_mul(angle).xreplace({omega: 1}) == 0
    assert C.masscenter.vel(N).simplify() == -omega * N.z
    assert C.masscenter.pos_from(P.masscenter) == N.x

    # Both joint pos id defined but different axes
    N, A, P, C = _generate_body()
    PinJoint('J',
             P,
             C,
             parent_joint_pos=N.x,
             child_joint_pos=A.x,
             child_axis=A.x + A.y)
    assert expand_mul(N.x.angle_between(A.x + A.y)) == 0  # Axis are aligned
    assert (A.x + A.y).express(N).simplify() == sqrt(2) * N.x
    assert A.dcm(N).simplify() == Matrix(
        [[sqrt(2) / 2, -sqrt(2) * cos(theta) / 2, -sqrt(2) * sin(theta) / 2],
         [sqrt(2) / 2,
          sqrt(2) * cos(theta) / 2,
          sqrt(2) * sin(theta) / 2], [0, -sin(theta),
                                      cos(theta)]])
    assert A.ang_vel_in(N) == omega * N.x
    assert (
        A.ang_vel_in(N).express(A).simplify() == (omega * A.x + omega * A.y) /
        sqrt(2))
    assert A.ang_vel_in(N).magnitude() == sqrt(omega**2)
    angle = A.ang_vel_in(N).angle_between(A.x + A.y)
    assert expand_mul(angle).xreplace({omega: 1}) == 0
    assert C.masscenter.vel(N).simplify() == (omega * A.z) / sqrt(2)
    assert C.masscenter.pos_from(P.masscenter) == N.x - A.x
    assert (C.masscenter.pos_from(
        P.masscenter).express(N).simplify() == (1 - sqrt(2) / 2) * N.x +
            sqrt(2) * cos(theta) / 2 * N.y + sqrt(2) * sin(theta) / 2 * N.z)
    assert (C.masscenter.vel(N).express(N).simplify() == -sqrt(2) * omega *
            sin(theta) / 2 * N.y + sqrt(2) * omega * cos(theta) / 2 * N.z)
    assert C.masscenter.vel(N).angle_between(A.x) == pi / 2

    N, A, P, C = _generate_body()
    PinJoint('J',
             P,
             C,
             parent_joint_pos=N.x,
             child_joint_pos=A.x,
             child_axis=A.x + A.y - A.z)
    assert expand_mul(N.x.angle_between(A.x + A.y - A.z)) == 0  # Axis aligned
    assert (A.x + A.y - A.z).express(N).simplify() == sqrt(3) * N.x
    assert A.dcm(N).simplify() == Matrix(
        [[
            sqrt(3) / 3, -sqrt(6) * sin(theta + pi / 4) / 3,
            sqrt(6) * cos(theta + pi / 4) / 3
        ],
         [
             sqrt(3) / 3,
             sqrt(6) * cos(theta + pi / 12) / 3,
             sqrt(6) * sin(theta + pi / 12) / 3
         ],
         [
             -sqrt(3) / 3,
             sqrt(6) * cos(theta + 5 * pi / 12) / 3,
             sqrt(6) * sin(theta + 5 * pi / 12) / 3
         ]])
    assert A.ang_vel_in(N) == omega * N.x
    assert A.ang_vel_in(N).express(
        A).simplify() == (omega * A.x + omega * A.y - omega * A.z) / sqrt(3)
    assert A.ang_vel_in(N).magnitude() == sqrt(omega**2)
    angle = A.ang_vel_in(N).angle_between(A.x + A.y - A.z)
    assert expand_mul(angle).xreplace({omega: 1}) == 0
    assert C.masscenter.vel(N).simplify() == (omega * A.y +
                                              omega * A.z) / sqrt(3)
    assert C.masscenter.pos_from(P.masscenter) == N.x - A.x
    assert (C.masscenter.pos_from(
        P.masscenter).express(N).simplify() == (1 - sqrt(3) / 3) * N.x +
            sqrt(6) * sin(theta + pi / 4) / 3 * N.y -
            sqrt(6) * cos(theta + pi / 4) / 3 * N.z)
    assert (C.masscenter.vel(N).express(N).simplify() == sqrt(6) * omega *
            cos(theta + pi / 4) / 3 * N.y +
            sqrt(6) * omega * sin(theta + pi / 4) / 3 * N.z)
    assert C.masscenter.vel(N).angle_between(A.x) == pi / 2

    N, A, P, C = _generate_body()
    m, n = symbols('m n')
    PinJoint('J',
             P,
             C,
             parent_joint_pos=m * N.x,
             child_joint_pos=n * A.x,
             child_axis=A.x + A.y - A.z,
             parent_axis=N.x - N.y + N.z)
    angle = (N.x - N.y + N.z).angle_between(A.x + A.y - A.z)
    assert expand_mul(angle) == 0  # Axis are aligned
    assert ((
        A.x - A.y +
        A.z).express(N).simplify() == (-4 * cos(theta) / 3 - 1 / 3) * N.x +
            (1 / 3 - 4 * sin(theta + pi / 6) / 3) * N.y +
            (4 * cos(theta + pi / 3) / 3 - 1 / 3) * N.z)
    assert A.dcm(N).simplify() == Matrix(
        [[
            S(1) / 3 - 2 * cos(theta) / 3,
            -2 * sin(theta + pi / 6) / 3 - S(1) / 3,
            2 * cos(theta + pi / 3) / 3 + S(1) / 3
        ],
         [
             2 * cos(theta + pi / 3) / 3 + S(1) / 3,
             2 * cos(theta) / 3 - S(1) / 3,
             2 * sin(theta + pi / 6) / 3 + S(1) / 3
         ],
         [
             -2 * sin(theta + pi / 6) / 3 - S(1) / 3,
             2 * cos(theta + pi / 3) / 3 + S(1) / 3,
             2 * cos(theta) / 3 - S(1) / 3
         ]])
    assert A.ang_vel_in(N) == (omega * N.x - omega * N.y +
                               omega * N.z) / sqrt(3)
    assert A.ang_vel_in(N).express(
        A).simplify() == (omega * A.x + omega * A.y - omega * A.z) / sqrt(3)
    assert A.ang_vel_in(N).magnitude() == sqrt(omega**2)
    angle = A.ang_vel_in(N).angle_between(A.x + A.y - A.z)
    assert expand_mul(angle).xreplace({omega: 1}) == 0
    assert (
        C.masscenter.vel(N).simplify() == (m * omega * N.y + m * omega * N.z +
                                           n * omega * A.y + n * omega * A.z) /
        sqrt(3))
    assert C.masscenter.pos_from(P.masscenter) == m * N.x - n * A.x
    assert (C.masscenter.pos_from(
        P.masscenter).express(N).simplify() == (m + n *
                                                (2 * cos(theta) - 1) / 3) * N.x
            + n * (2 * sin(theta + pi / 6) + 1) / 3 * N.y - n *
            (2 * cos(theta + pi / 3) + 1) / 3 * N.z)
    assert (C.masscenter.vel(N).express(N).simplify() == -2 * n * omega *
            sin(theta) / 3 * N.x +
            (sqrt(3) * m + 2 * n * cos(theta + pi / 6)) * omega / 3 * N.y +
            (sqrt(3) * m + 2 * n * sin(theta + pi / 3)) * omega / 3 * N.z)
    assert expand_mul(C.masscenter.vel(N).angle_between(m * N.x -
                                                        n * A.x)) == pi / 2
Пример #51
0
def test_sub_qdot2():
    # This test solves exercises 8.3 from Kane 1985 and defines
    # all velocities in terms of q, qdot. We check that the generalized active
    # forces are correctly computed if u terms are only defined in the
    # kinematic differential equations.
    #
    # This functionality was added in PR 8948. Without qdot/u substitution, the
    # KanesMethod constructor will fail during the constraint initialization as
    # the B matrix will be poorly formed and inversion of the dependent part
    # will fail.

    g, m, Px, Py, Pz, R, t = symbols('g m Px Py Pz R t')
    q = dynamicsymbols('q:5')
    qd = dynamicsymbols('q:5', level=1)
    u = dynamicsymbols('u:5')

    ## Define inertial, intermediate, and rigid body reference frames
    A = ReferenceFrame('A')
    B_prime = A.orientnew('B_prime', 'Axis', [q[0], A.z])
    B = B_prime.orientnew('B', 'Axis', [pi / 2 - q[1], B_prime.x])
    C = B.orientnew('C', 'Axis', [q[2], B.z])

    ## Define points of interest and their velocities
    pO = Point('O')
    pO.set_vel(A, 0)

    # R is the point in plane H that comes into contact with disk C.
    pR = pO.locatenew('R', q[3] * A.x + q[4] * A.y)
    pR.set_vel(A, pR.pos_from(pO).diff(t, A))
    pR.set_vel(B, 0)

    # C^ is the point in disk C that comes into contact with plane H.
    pC_hat = pR.locatenew('C^', 0)
    pC_hat.set_vel(C, 0)

    # C* is the point at the center of disk C.
    pCs = pC_hat.locatenew('C*', R * B.y)
    pCs.set_vel(C, 0)
    pCs.set_vel(B, 0)

    # calculate velocites of points C* and C^ in frame A
    pCs.v2pt_theory(pR, A, B)  # points C* and R are fixed in frame B
    pC_hat.v2pt_theory(pCs, A, C)  # points C* and C^ are fixed in frame C

    ## Define forces on each point of the system
    R_C_hat = Px * A.x + Py * A.y + Pz * A.z
    R_Cs = -m * g * A.z
    forces = [(pC_hat, R_C_hat), (pCs, R_Cs)]

    ## Define kinematic differential equations
    # let ui = omega_C_A & bi (i = 1, 2, 3)
    # u4 = qd4, u5 = qd5
    u_expr = [C.ang_vel_in(A) & uv for uv in B]
    u_expr += qd[3:]
    kde = [ui - e for ui, e in zip(u, u_expr)]
    km1 = KanesMethod(A, q, u, kde)
    with warns_deprecated_sympy():
        fr1, _ = km1.kanes_equations(forces, [])

    ## Calculate generalized active forces if we impose the condition that the
    # disk C is rolling without slipping
    u_indep = u[:3]
    u_dep = list(set(u) - set(u_indep))
    vc = [pC_hat.vel(A) & uv for uv in [A.x, A.y]]
    km2 = KanesMethod(A,
                      q,
                      u_indep,
                      kde,
                      u_dependent=u_dep,
                      velocity_constraints=vc)
    with warns_deprecated_sympy():
        fr2, _ = km2.kanes_equations(forces, [])

    fr1_expected = Matrix([
        -R * g * m * sin(q[1]),
        -R * (Px * cos(q[0]) + Py * sin(q[0])) * tan(q[1]),
        R * (Px * cos(q[0]) + Py * sin(q[0])), Px, Py
    ])
    fr2_expected = Matrix([-R * g * m * sin(q[1]), 0, 0])
    assert (trigsimp(fr1.expand()) == trigsimp(fr1_expected.expand()))
    assert (trigsimp(fr2.expand()) == trigsimp(fr2_expected.expand()))
Пример #52
0
    def __init__(self):

        super(Quad, self).__init__()

        # states
        phi, theta, psi_, P, Q, R, x, y, z, U, V, W = mech.dynamicsymbols('phi, theta, psi_, P, Q, R, x, y, z, U, V, W')
        self.x = sympy.Matrix([phi, theta, psi_, P, Q, R, x, y, z, U, V, W])
        self.x0 = {
            phi : 0,
            theta : 0,
            psi_ : 0,
            P : 0,
            Q : 0,
            R : 0,
            x : 0,
            y : 0,
            z : 0,
            U : 0,
            V : 0,
            W : 0,
            }

        # variables
        F_x, F_y, F_z, M_x, M_y, M_z = mech.dynamicsymbols('F_x, F_y, F_z, M_x, M_y, M_z')
        self.v = sympy.Matrix([F_x, F_y, F_z, M_x, M_y, M_z])

        # constants
        self.c = sympy.Matrix([])
        self.c0 = {
            }

        # parameters
        J_x, J_y, J_z, m = sympy.symbols('J_x, J_y, J_z, m')
        self.p = sympy.Matrix([J_x, J_y, J_z, m])
        self.p0 = {
            J_x : 1.0,
            J_y : 1.0,
            J_z : 1.0,
            m : 1.0,
            }

        # inputs
        self.u = sympy.Matrix([])
        self.u0 = {
            }

        # outputs
        self.y = sympy.Matrix([])

        # equations
        self.eqs = [
            M_x - (- P - phi),
            M_y - (- Q - theta),
            M_z - (- R - psi_),
            F_x - (- x),
            F_y - (- y),
            F_z - (- z),
            (x).diff(self.t) - (U),
            (y).diff(self.t) - (V),
            (z).diff(self.t) - (W),
            - m * V * (R).diff(self.t) + m * W * (Q).diff(self.t) + m * (U).diff(self.t) - (F_x),
            m * U * (R).diff(self.t) - m * W * (P).diff(self.t) + m * (V).diff(self.t) - (F_y),
            - m * U * (Q).diff(self.t) + m * V * (P).diff(self.t) + m * (W).diff(self.t) - (F_z),
            (phi).diff(self.t) - (P + Q * sin(phi) * tan(theta) + R * cos(phi) * tan(theta)),
            (theta).diff(self.t) - (Q * cos(phi) - R * sin(phi)),
            cos(theta) * (psi_).diff(self.t) - (Q * sin(phi) + R * cos(phi)),
            (P).diff(self.t) - (M_x),
            (Q).diff(self.t) - (M_y),
            (R).diff(self.t) - (M_z),
            ]

        self.compute_fg()
Пример #53
-1
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([])
Пример #54
-1
def test_nonminimal_pendulum():
    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)
    # 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()
    # Check solution
    lam1 = LM.lam_vec[0, 0]
    eom_sol = Matrix([[m*Derivative(q1, t, t) - 9.8*m + 2*lam1*q1],
                      [m*Derivative(q2, t, t) + 2*lam1*q2]])
    assert LM.eom == eom_sol
    # Check multiplier solution
    lam_sol = Matrix([(19.6*q1 + 2*q1d**2 + 2*q2d**2)/(4*q1**2/m + 4*q2**2/m)])
    assert LM.solve_multipliers(sol_type='Matrix') == lam_sol
Пример #55
-1
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])
Пример #56
-1
    def setup(self):
        # System state variables
        q = me.dynamicsymbols('q')
        qd = me.dynamicsymbols('q', 1)

        # Other system variables
        m, k, b = symbols('m k b')

        # Set up the reference frame
        N = me.ReferenceFrame('N')

        # Set up the points and particles
        P = me.Point('P')
        P.set_vel(N, qd * N.x)

        Pa = me.Particle('Pa', P, m)

        # Define the potential energy and create the Lagrangian
        Pa.potential_energy = k * q**2 / 2.0
        L = me.Lagrangian(N, Pa)

        # Create the list of forces acting on the system
        fl = [(P, -b * qd * N.x)]

        # Create an instance of Lagranges Method
        self.l = me.LagrangesMethod(L, [q], forcelist=fl, frame=N)
Пример #57
-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)
    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 (KM.linearize(A_and_B=True, new_method=True)[0] ==
            Matrix([[0, 1], [-k/m, -c/m]]))

    # Ensure that the old linearizer still works and that the new linearizer
    # gives the same results. The old linearizer is deprecated and should be
    # removed in >= 0.7.7.
    M_old = KM.mass_matrix_full
    # The old linearizer 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)
        F_A_old, F_B_old, r_old = KM.linearize()
    M_new, F_A_new, F_B_new, r_new = KM.linearize(new_method=True)
    assert simplify(M_new.inv() * F_A_new - M_old.inv() * F_A_old) == zeros(2)
Пример #58
-1
def test_point_funcs():
    q, q2 = dynamicsymbols('q q2')
    qd, q2d = dynamicsymbols('q q2', 1)
    qdd, q2dd = dynamicsymbols('q q2', 2)
    N = ReferenceFrame('N')
    B = ReferenceFrame('B')
    B.set_ang_vel(N, 5 * B.y)
    O = Point('O')
    P = O.locatenew('P', q * B.x)
    assert P.pos_from(O) == q * B.x
    P.set_vel(B, qd * B.x + q2d * B.y)
    assert P.vel(B) == qd * B.x + q2d * B.y
    O.set_vel(N, 0)
    assert O.vel(N) == 0
    assert P.a1pt_theory(O, N, B) == ((-25 * q + qdd) * B.x + (q2dd) * B.y +
                               (-10 * qd) * B.z)

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

    B.set_ang_vel(N, 5 * B.y)
    O = Point('O')
    P = O.locatenew('P', q * B.x)
    P.set_vel(B, qd * B.x + q2d * B.y)
    O.set_vel(N, 0)
    assert P.v1pt_theory(O, N, B) == qd * B.x + q2d * B.y - 5 * q * B.z
Пример #59
-1
def test_partial_velocity():
    q1, q2, q3, u1, u2, u3 = dynamicsymbols("q1 q2 q3 u1 u2 u3")
    u4, u5 = dynamicsymbols("u4, u5")
    r = symbols("r")

    N = ReferenceFrame("N")
    Y = N.orientnew("Y", "Axis", [q1, N.z])
    L = Y.orientnew("L", "Axis", [q2, Y.x])
    R = L.orientnew("R", "Axis", [q3, L.y])
    R.set_ang_vel(N, u1 * L.x + u2 * L.y + u3 * L.z)

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

    vel_list = [Dmc.vel(N), C.vel(N), R.ang_vel_in(N)]
    u_list = [u1, u2, u3, u4, u5]
    assert partial_velocity(vel_list, u_list) == [
        [-r * L.y, 0, L.x],
        [r * L.x, 0, L.y],
        [0, 0, L.z],
        [L.x, L.x, 0],
        [cos(q2) * L.y - sin(q2) * L.z, cos(q2) * L.y - sin(q2) * L.z, 0],
    ]
Пример #60
-1
 def __init__(self, coordinate_name, *args, **kwargs):
     super(RevoluteJoint, self).__init__(*args, **kwargs)
     # TODO where should these be stored?
     # TODO this would manage creating an extra speed for a quaternion.
     # TODO ensure coordinate names are not getting overwritten.
     self._rotation = dynamicsymbols(coordinate_name)
     self._rotationdot = dynamicsymbols(coordinate_name, 1)
     self._rotspeed = dynamicsymbols('%s_u' % coordinate_name)
     self._rotspeeddot = dynamicsymbols('%s_u' % coordinate_name, 1)