Python KanesMethod.kindiffdict方法代码示例

# 需要导入模块: from sympy.physics.mechanics import KanesMethod [as 别名]
# 或者: from sympy.physics.mechanics.KanesMethod import kindiffdict [as 别名]
def get_equations(m_val, g_val, l_val):
    # This function body is copyied from:
    # http://www.pydy.org/examples/double_pendulum.html
    # Retrieved 2015-09-29
    from sympy import symbols
    from sympy.physics.mechanics import (
        dynamicsymbols, ReferenceFrame, Point, Particle, KanesMethod
    )

    q1, q2 = dynamicsymbols('q1 q2')
    q1d, q2d = dynamicsymbols('q1 q2', 1)
    u1, u2 = dynamicsymbols('u1 u2')
    u1d, u2d = dynamicsymbols('u1 u2', 1)
    l, m, g = symbols('l m g')

    N = ReferenceFrame('N')
    A = N.orientnew('A', 'Axis', [q1, N.z])
    B = N.orientnew('B', 'Axis', [q2, N.z])

    A.set_ang_vel(N, u1 * N.z)
    B.set_ang_vel(N, u2 * N.z)

    O = Point('O')
    P = O.locatenew('P', l * A.x)
    R = P.locatenew('R', l * B.x)

    O.set_vel(N, 0)
    P.v2pt_theory(O, N, A)
    R.v2pt_theory(P, N, B)

    ParP = Particle('ParP', P, m)
    ParR = Particle('ParR', R, m)

    kd = [q1d - u1, q2d - u2]
    FL = [(P, m * g * N.x), (R, m * g * N.x)]
    BL = [ParP, ParR]

    KM = KanesMethod(N, q_ind=[q1, q2], u_ind=[u1, u2], kd_eqs=kd)

    (fr, frstar) = KM.kanes_equations(FL, BL)
    kdd = KM.kindiffdict()
    mm = KM.mass_matrix_full
    fo = KM.forcing_full
    qudots = mm.inv() * fo
    qudots = qudots.subs(kdd)
    qudots.simplify()
    # Edit:
    depv = [q1, q2, u1, u2]
    subs = list(zip([m, g, l], [m_val, g_val, l_val]))
    return zip(depv, [expr.subs(subs) for expr in qudots])

# 需要导入模块: from sympy.physics.mechanics import KanesMethod [as 别名]
# 或者: from sympy.physics.mechanics.KanesMethod import kindiffdict [as 别名]
def test_rolling_disc():
    # Rolling Disc Example
    # Here the rolling disc is formed from the contact point up, removing the
    # need to introduce generalized speeds. Only 3 configuration and 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)
    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()

    # 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, new_method=True)[0]
    A_upright = A.subs({r: 1, g: 1, m: 1}).subs({q1: 0, q2: 0, q3: 0, u1: 0, u3: 0})
    assert A_upright.subs(u2, 1 / sqrt(3)).eigenvals() == {S(0): 6}

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

#.........这里部分代码省略.........
    #                                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))
    #u_dep_dict = {udi : u_depi[0] for udi, u_depi in zip(u[3:], u_dep.tolist())}

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


    # Test
    # First try Fr_c == fr;
    # Second try Fr_star_c == frstar;
    # Third try Fr_star_steady == frstar_steady.
    # Both signs are checked in case the equations were found with an inverse
    # sign.
    assert ((Matrix(Fr_c).expand() == fr.expand()) or
             (Matrix(Fr_c).expand() == (-fr).expand()))

    assert ((Matrix(Fr_star_c).expand() == frstar.expand()) or
             (Matrix(Fr_star_c).expand() == (-frstar).expand()))

    assert ((Matrix(Fr_star_steady).expand() == frstar_steady.expand()) or
             (Matrix(Fr_star_steady).expand() == (-frstar_steady).expand()))

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

#.........这里部分代码省略.........
    PaperForkCgX                    =  0.9
    PaperForkCgZ                    =  0.7
    FrameLength                     =  evalf.N(PaperWb*sin(HTA)-(rake-(PaperRadFront-PaperRadRear)*cos(HTA)))
    FrameCGNorm                     =  evalf.N((PaperFrameCgZ - PaperRadRear-(PaperFrameCgX/sin(HTA))*cos(HTA))*sin(HTA))
    FrameCGPar                      =  evalf.N((PaperFrameCgX / sin(HTA) + (PaperFrameCgZ - PaperRadRear - PaperFrameCgX / sin(HTA) * cos(HTA)) * cos(HTA)))
    tempa                           =  evalf.N((PaperForkCgZ - PaperRadFront))
    tempb                           =  evalf.N((PaperWb-PaperForkCgX))
    tempc                           =  evalf.N(sqrt(tempa**2+tempb**2))
    PaperForkL                      =  evalf.N((PaperWb*cos(HTA)-(PaperRadFront-PaperRadRear)*sin(HTA)))
    ForkCGNorm                      =  evalf.N(rake+(tempc * sin(pi/2-HTA-acos(tempa/tempc))))
    ForkCGPar                       =  evalf.N(tempc * cos((pi/2-HTA)-acos(tempa/tempc))-PaperForkL)

    # Here is the final assembly of the numerical values. The symbol 'v' is the
    # forward speed of the bicycle (a concept which only makes sense in the
    # upright, static equilibrium case?). These are in a dictionary which will
    # later be substituted in. Again the sign on the *product* of inertia
    # values is flipped here, due to different orientations of coordinate
    # systems.
    v = symbols('v')
    val_dict = {WFrad: PaperRadFront,
                WRrad: PaperRadRear,
                htangle: HTA,
                forkoffset: rake,
                forklength: PaperForkL,
                framelength: FrameLength,
                forkcg1: ForkCGPar,
                forkcg3: ForkCGNorm,
                framecg1: FrameCGNorm,
                framecg3: FrameCGPar,
                Iwr11: 0.0603,
                Iwr22: 0.12,
                Iwf11: 0.1405,
                Iwf22: 0.28,
                Ifork11: 0.05892,
                Ifork22: 0.06,
                Ifork33: 0.00708,
                Ifork31: 0.00756,
                Iframe11: 9.2,
                Iframe22: 11,
                Iframe33: 2.8,
                Iframe31: -2.4,
                mfork: 4,
                mframe: 85,
                mwf: 3,
                mwr: 2,
                g: 9.81,
                q1: 0,
                q2: 0,
                q4: 0,
                q5: 0,
                u1: 0,
                u2: 0,
                u3: v / PaperRadRear,
                u4: 0,
                u5: 0,
                u6: v / PaperRadFront}

    # Linearizes the forcing vector; the equations are set up as MM udot =
    # forcing, where MM is the mass matrix, udot is the vector representing the
    # time derivatives of the generalized speeds, and forcing is a vector which
    # contains both external forcing terms and internal forcing terms, such as
    # centripital or coriolis forces.  This actually returns a matrix with as
    # many rows as *total* coordinates and speeds, but only as many columns as
    # independent coordinates and speeds.

    forcing_lin = KM.linearize()[0]

    # As mentioned above, the size of the linearized forcing terms is expanded
    # to include both q's and u's, so the mass matrix must have this done as
    # well.  This will likely be changed to be part of the linearized process,
    # for future reference.
    MM_full = KM.mass_matrix_full

    MM_full_s = MM_full.subs(val_dict)
    forcing_lin_s = forcing_lin.subs(KM.kindiffdict()).subs(val_dict)


    MM_full_s = MM_full_s.evalf()
    forcing_lin_s = forcing_lin_s.evalf()

    # Finally, we construct an "A" matrix for the form xdot = A x (x being the
    # state vector, although in this case, the sizes are a little off). The
    # following line extracts only the minimum entries required for eigenvalue
    # analysis, which correspond to rows and columns for lean, steer, lean
    # rate, and steer rate.
    Amat = MM_full_s.inv() * forcing_lin_s
    A = Amat.extract([1, 2, 4, 6], [1, 2, 3, 5])

    # Precomputed for comparison
    Res = Matrix([[               0,                                           0,                  1.0,                    0],
                  [               0,                                           0,                    0,                  1.0],
                  [9.48977444677355, -0.891197738059089*v**2 - 0.571523173729245, -0.105522449805691*v, -0.330515398992311*v],
                  [11.7194768719633,   -1.97171508499972*v**2 + 30.9087533932407,   3.67680523332152*v,  -3.08486552743311*v]])


    # Actual eigenvalue comparison
    eps = 1.e-12
    for i in range(6):
        error = Res.subs(v, i) - A.subs(v, i)
        assert all(abs(x) < eps for x in error)

# 需要导入模块: from sympy.physics.mechanics import KanesMethod [as 别名]
# 或者: from sympy.physics.mechanics.KanesMethod import kindiffdict [as 别名]
class Linkage(MultiBodySystem):
    """TODO
    """

    def __init__(self, name):
        self._name = name
        self._root = RootLink(self) # TODO maybe don't need backpointer
        self._constants = dict()
        self._constant_descs = dict()
        self._forces = dict()
        self._gravity_vector = None
        self._gravity_vector_updated = Event(
                'Update gravity forces when gravity vector is changed.')
        self._gravity_vector_updated.subscriber_new(self._manage_gravity_forces)

    def _get_name(self):
        return self._name
    def _set_name(self, name):
        self._name = name
    name = property(_get_name, _set_name)

    def _get_root(self):
        return self._root
    root = property(_get_root)

    def constant_new(self, name, description):
        self._constants[name] = symbols(name)
        self._constant_descs[name] = description
        return self._constants[name]

    def force_new(self, name, point_of_application, vec):
        """TODO
        reserved names: '_gravity'
        """
        # TODO 
        #if name in self._forces:
        #    # TODO
        #    raise Exception("Force with name '{}' already exists.".format(name))
        self._forces[name] = (point_of_application, vec)

    def force_del(self, name):
        self._forces.pop(name)

    def _manage_gravity_forces(self):
        # TODO must modify to account for gravity_force being modified more
        # than once.
        for body in self.body_list():
            self.force_new('%s_gravity', body.masscenter, body.mass * self.gravity_vector)

    def _get_gravity_vector(self):
        return self._gravity_vector
    def _set_gravity_vector(self, vec):
        _check_vector(vec)
        self._gravity_vector = vec
        self._gravity_vector_updated.fire()
    gravity_vector = property(_get_gravity_vector, _set_gravity_vector)

    def independent_coordinates(self):
        return self.root.independent_coordinates_in_subtree()

    def independent_speeds(self):
        return self.root.independent_speeds_in_subtree()

    def kinematic_differential_equations(self):
        return self.root.kinematic_differential_equations_in_subtree()

    def body_list(self):
        return self.root.body_list_in_subtree()

    def force_list(self):
        return self._forces.values()

    #TODO def _init_kanes_method(self):
    #TODO     # TODO move the creation of Kane's Method somewhere else.
    #TODO     self._kanes_method = KanesMethod(self.independent_coordinates,
    #TODO             self.independent_speeds, self.kinematic_diffeqs)
    #TODO     # TODO must make this call to get the mass matrix, etc.?
    #TODO     self._kanes_method.kanes_equations(self.force_list, self.body_list)

    def mass_matrix(self):
        #if not (self._kanes_method and self.up_to_date):
        #    self._init_kanes_method()
        # TODO move the creation of Kane's Method somewhere else.
        self._kanes_method = KanesMethod(self.root.frame,
                q_ind=self.independent_coordinates(),
                u_ind=self.independent_speeds(),
                kd_eqs=self.kinematic_differential_equations()
                )
        # TODO must make this call to get the mass matrix, etc.?
        self._kanes_method.kanes_equations(self.force_list(), self.body_list());
        return self._kanes_method.mass_matrix

    def state_derivatives(self):
        # TODO find a way to use a cached mass matrix.
        kin_diff_eqns = self._kanes_method.kindiffdict()
        state_derivatives = self.mass_matrix.inv() * self._kanes_method.forcing
        state_derivatives = state_derivatives.subs(kin_diff_eqns)
        state_derivatives.simplify()
        return state_derivatives

#.........这里部分代码省略.........