def _initialize_vectors(self, q_ind, q_dep, u_ind, u_dep, u_aux):
        """Initialize the coordinate and speed vectors."""

        none_handler = lambda x: Matrix(x) if x else Matrix()

        # Initialize generalized coordinates
        q_dep = none_handler(q_dep)
        if not iterable(q_ind):
            raise TypeError('Generalized coordinates must be an iterable.')
        if not iterable(q_dep):
            raise TypeError('Dependent coordinates must be an iterable.')
        q_ind = Matrix(q_ind)
        self._qdep = q_dep
        self._q = Matrix([q_ind, q_dep])
        self._qdot = self.q.diff(dynamicsymbols._t)

        # Initialize generalized speeds
        u_dep = none_handler(u_dep)
        if not iterable(u_ind):
            raise TypeError('Generalized speeds must be an iterable.')
        if not iterable(u_dep):
            raise TypeError('Dependent speeds must be an iterable.')
        u_ind = Matrix(u_ind)
        self._udep = u_dep
        self._u = Matrix([u_ind, u_dep])
        self._udot = self.u.diff(dynamicsymbols._t)
        self._uaux = none_handler(u_aux)

 def _contains(self, other):
     from sympy.matrices import Matrix
     from sympy.solvers.solveset import solveset, linsolve
     from sympy.utilities.iterables import iterable, cartes
     L = self.lamda
     if self._is_multivariate():
         if not iterable(L.expr):
             if iterable(other):
                 return S.false
             return other.as_numer_denom() in self.func(
                 Lambda(L.variables, L.expr.as_numer_denom()), self.base_set)
         if len(L.expr) != len(self.lamda.variables):
             raise NotImplementedError(filldedent('''
 Dimensions of input and output of Lambda are different.'''))
         eqs = [expr - val for val, expr in zip(other, L.expr)]
         variables = L.variables
         free = set(variables)
         if all(i.is_number for i in list(Matrix(eqs).jacobian(variables))):
             solns = list(linsolve([e - val for e, val in
             zip(L.expr, other)], variables))
         else:
             syms = [e.free_symbols & free for e in eqs]
             solns = {}
             for i, (e, s, v) in enumerate(zip(eqs, syms, other)):
                 if not s:
                     if e != v:
                         return S.false
                     solns[vars[i]] = [v]
                     continue
                 elif len(s) == 1:
                     sy = s.pop()
                     sol = solveset(e, sy)
                     if sol is S.EmptySet:
                         return S.false
                     elif isinstance(sol, FiniteSet):
                         solns[sy] = list(sol)
                     else:
                         raise NotImplementedError
                 else:
                     raise NotImplementedError
             solns = cartes(*[solns[s] for s in variables])
     else:
         # assume scalar -> scalar mapping
         solnsSet = solveset(L.expr - other, L.variables[0])
         if solnsSet.is_FiniteSet:
             solns = list(solnsSet)
         else:
             raise NotImplementedError(filldedent('''
             Determining whether an ImageSet contains %s has not
             been implemented.''' % func_name(other)))
     for soln in solns:
         try:
             if soln in self.base_set:
                 return S.true
         except TypeError:
             return self.base_set.contains(soln.evalf())
     return S.false

 def __new__(cls, target, iter, body):
     target = _sympify(target)
     if not iterable(iter):
         raise TypeError("iter must be an iterable")
     iter = _sympify(iter)
     if not iterable(body):
         raise TypeError("body must be an iterable")
     body = Tuple(*(_sympify(i) for i in body))
     return Basic.__new__(cls, target, iter, body)

def partial_velocity(vel_vecs, gen_speeds, frame):
    """Returns a list of partial velocities with respect to the provided
    generalized speeds in the given reference frame for each of the supplied
    velocity vectors.

    The output is a list of lists. The outer list has a number of elements
    equal to the number of supplied velocity vectors. The inner lists are, for
    each velocity vector, the partial derivatives of that velocity vector with
    respect to the generalized speeds supplied.

    Parameters
    ==========

    vel_vecs : iterable
        An iterable of velocity vectors (angular or linear).
    gen_speeds : iterable
        An iterable of generalized speeds.
    frame : ReferenceFrame
        The reference frame that the partial derivatives are going to be taken
        in.

    Examples
    ========

    >>> from sympy.physics.vector import Point, ReferenceFrame
    >>> from sympy.physics.vector import dynamicsymbols
    >>> from sympy.physics.vector import partial_velocity
    >>> u = dynamicsymbols('u')
    >>> N = ReferenceFrame('N')
    >>> P = Point('P')
    >>> P.set_vel(N, u * N.x)
    >>> vel_vecs = [P.vel(N)]
    >>> gen_speeds = [u]
    >>> partial_velocity(vel_vecs, gen_speeds, N)
    [[N.x]]

    """

    if not iterable(vel_vecs):
        raise TypeError('Velocity vectors must be contained in an iterable.')

    if not iterable(gen_speeds):
        raise TypeError('Generalized speeds must be contained in an iterable')

    vec_partials = []
    for vec in vel_vecs:
        partials = []
        for speed in gen_speeds:
            partials.append(vec.diff(speed, frame, var_in_dcm=False))
        vec_partials.append(partials)

    return vec_partials

 def __new__(cls, target, iter, body):
     target = _sympify(target)
     if not iterable(iter):
         raise TypeError("iter must be an iterable")
     if isinstance(iter, list):
         # _sympify errors on lists because they are mutable
         iter = tuple(iter)
     iter = _sympify(iter)
     if not isinstance(body, CodeBlock):
         if not iterable(body):
             raise TypeError("body must be an iterable or CodeBlock")
         body = CodeBlock(*(_sympify(i) for i in body))
     return Basic.__new__(cls, target, iter, body)

def partial_velocity(vel_list, u_list, frame):
    """Returns a list of partial velocities.

    For a list of velocity or angular velocity vectors the partial derivatives
    with respect to the supplied generalized speeds are computed, in the
    specified ReferenceFrame.

    The output is a list of lists. The outer list has a number of elements
    equal to the number of supplied velocity vectors. The inner lists are, for
    each velocity vector, the partial derivatives of that velocity vector with
    respect to the generalized speeds supplied.

    Parameters
    ==========

    vel_list : list
        List of velocities of Point's and angular velocities of ReferenceFrame's
    u_list : list
        List of independent generalized speeds.
    frame : ReferenceFrame
        The ReferenceFrame the partial derivatives are going to be taken in.

    Examples
    ========

    >>> from sympy.physics.vector import Point, ReferenceFrame
    >>> from sympy.physics.vector import dynamicsymbols
    >>> from sympy.physics.vector import partial_velocity
    >>> u = dynamicsymbols('u')
    >>> N = ReferenceFrame('N')
    >>> P = Point('P')
    >>> P.set_vel(N, u * N.x)
    >>> vel_list = [P.vel(N)]
    >>> u_list = [u]
    >>> partial_velocity(vel_list, u_list, N)
    [[N.x]]

    """
    if not iterable(vel_list):
        raise TypeError('Provide velocities in an iterable')
    if not iterable(u_list):
        raise TypeError('Provide speeds in an iterable')
    list_of_pvlists = []
    for i in vel_list:
        pvlist = []
        for j in u_list:
            vel = i.diff(j, frame)
            pvlist += [vel]
        list_of_pvlists += [pvlist]
    return list_of_pvlists

def dynamicsymbols(names, level=0):
    """Uses symbols and Function for functions of time.

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

    Parameters
    ==========

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

    Examples
    ========

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

    """
    esses = symbols(names, cls=Function)
    t = dynamicsymbols._t
    if iterable(esses):
        esses = [reduce(diff, [t] * level, e(t)) for e in esses]
        return esses
    else:
        return reduce(diff, [t] * level, esses(t))

    def _form_fr(self, fl):
        """Form the generalized active force."""
        if fl != None and (len(fl) == 0 or not iterable(fl)):
            raise ValueError('Force pairs must be supplied in an '
                'non-empty iterable or None.')

        N = self._inertial
        # pull out relevant velocities for constructing partial velocities
        vel_list, f_list = _f_list_parser(fl, N)
        vel_list = [msubs(i, self._qdot_u_map) for i in vel_list]

        # Fill Fr with dot product of partial velocities and forces
        o = len(self.u)
        b = len(f_list)
        FR = zeros(o, 1)
        partials = partial_velocity(vel_list, self.u, N)
        for i in range(o):
            FR[i] = sum(partials[j][i] & f_list[j] for j in range(b))

        # In case there are dependent speeds
        if self._udep:
            p = o - len(self._udep)
            FRtilde = FR[:p, 0]
            FRold = FR[p:o, 0]
            FRtilde += self._Ars.T * FRold
            FR = FRtilde

        self._forcelist = fl
        self._fr = FR
        return FR

def dimension(*args):
    """ Creates a 'dimension' Attribute with (up to 7) extents.

    Examples
    ========

    >>> from sympy.printing import fcode
    >>> from sympy.codegen.fnodes import dimension, intent_in
    >>> dim = dimension('2', ':')  # 2 rows, runtime determined number of columns
    >>> from sympy.codegen.ast import Variable, integer
    >>> arr = Variable('a', integer, attrs=[dim, intent_in])
    >>> fcode(arr.as_Declaration(), source_format='free', standard=2003)
    'integer*4, dimension(2, :), intent(in) :: a'

    """
    if len(args) > 7:
        raise ValueError("Fortran only supports up to 7 dimensional arrays")
    parameters = []
    for arg in args:
        if isinstance(arg, Extent):
            parameters.append(arg)
        elif isinstance(arg, string_types):
            if arg == ':':
                parameters.append(Extent())
            else:
                parameters.append(String(arg))
        elif iterable(arg):
            parameters.append(Extent(*arg))
        else:
            parameters.append(sympify(arg))
    if len(args) == 0:
        raise ValueError("Need at least one dimension")
    return Attribute('dimension', parameters)

def find_dynamicsymbols(expression, exclude=None):
    """Find all dynamicsymbols in expression.

    >>> from sympy.physics.mechanics import dynamicsymbols, find_dynamicsymbols
    >>> x, y = dynamicsymbols('x, y')
    >>> expr = x + x.diff()*y
    >>> find_dynamicsymbols(expr)
    set([x(t), y(t), Derivative(x(t), t)])

    If the optional ``exclude`` kwarg is used, only dynamicsymbols
    not in the iterable ``exclude`` are returned.

    >>> find_dynamicsymbols(expr, [x, y])
    set([Derivative(x(t), t)])
    """
    t_set = set([dynamicsymbols._t])
    if exclude:
        if iterable(exclude):
            exclude_set = set(exclude)
        else:
            raise TypeError("exclude kwarg must be iterable")
    else:
        exclude_set = set()
    return set([i for i in expression.atoms(AppliedUndef, Derivative) if
            i.free_symbols == t_set]) - exclude_set

 def __new__(cls, name, body):
     if not isinstance(name, str):
         raise TypeError("Module name must be string")
     name = Symbol(name)
     # body
     if not iterable(body):
         raise TypeError("body must be an iterable")
     body = Tuple(*body)
     return Basic.__new__(cls, name, body)

 def __new__(cls, name, args, results):
     # name
     if isinstance(name, str):
         name = Symbol(name)
     elif not isinstance(name, Symbol):
         raise TypeError("Function name must be Symbol or string")
     # args
     if not iterable(args):
         raise TypeError("args must be an iterable")
     if not all(isinstance(a, Argument) for a in args):
         raise TypeError("All args must be of type Argument")
     args = Tuple(*args)
     # results
     if not iterable(results):
         raise TypeError("results must be an iterable")
     if not all(isinstance(i, RoutineResult) for i in results):
         raise TypeError("All results must be of type RoutineResult")
     results = Tuple(*results)
     return Basic.__new__(cls, name, args, results)

 def __new__(cls, *args):
     if len(args)==1 and args[0].__class__ is dict:
         items = [Tuple(k, v) for k, v in args[0].iteritems()]
     elif iterable(args) and all(len(arg) == 2 for arg in args):
         items = [Tuple(k, v) for k, v in args]
     else:
         raise TypeError('Pass Dict args as Dict((k1, v1), ...) or Dict({k1: v1, ...})')
     obj = Basic.__new__(cls, *items)
     obj._dict = dict(items) # In case Tuple decides it wants to sympify
     return obj

def find_dynamicsymbols(expression, exclude=None, reference_frame=None):
    """Find all dynamicsymbols in expression.

    If the optional ``exclude`` kwarg is used, only dynamicsymbols
    not in the iterable ``exclude`` are returned.
    If we intend to apply this function on a vector, the optional
    ''reference_frame'' is also used to inform about the corresponding frame
    with respect to which the dynamic symbols of the given vector is to be
    determined.

    Parameters
    ==========

    expression : sympy expression

    exclude : iterable of dynamicsymbols, optional

    reference_frame : ReferenceFrame, optional
        The frame with respect to which the dynamic symbols of the
        given vector is to be determined.

    Examples
    ========

    >>> from sympy.physics.mechanics import dynamicsymbols, find_dynamicsymbols
    >>> from sympy.physics.mechanics import ReferenceFrame
    >>> x, y = dynamicsymbols('x, y')
    >>> expr = x + x.diff()*y
    >>> find_dynamicsymbols(expr)
    {x(t), y(t), Derivative(x(t), t)}
    >>> find_dynamicsymbols(expr, exclude=[x, y])
    {Derivative(x(t), t)}
    >>> a, b, c = dynamicsymbols('a, b, c')
    >>> A = ReferenceFrame('A')
    >>> v = a * A.x + b * A.y + c * A.z
    >>> find_dynamicsymbols(v, reference_frame=A)
    {a(t), b(t), c(t)}

    """
    t_set = {dynamicsymbols._t}
    if exclude:
        if iterable(exclude):
            exclude_set = set(exclude)
        else:
            raise TypeError("exclude kwarg must be iterable")
    else:
        exclude_set = set()
    if isinstance(expression, Vector):
        if reference_frame is None:
            raise ValueError("You must provide reference_frame when passing a "
                             "vector expression, got %s." % reference_frame)
        else:
            expression = expression.to_matrix(reference_frame)
    return set([i for i in expression.atoms(AppliedUndef, Derivative) if
            i.free_symbols == t_set]) - exclude_set

 def __new__(cls, *args):
     if len(args) == 1 and isinstance(args[0], (dict, Dict)):
         items = [Tuple(k, v) for k, v in args[0].items()]
     elif iterable(args) and all(len(arg) == 2 for arg in args):
         items = [Tuple(k, v) for k, v in args]
     else:
         raise TypeError('Pass Dict args as Dict((k1, v1), ...) or Dict({k1: v1, ...})')
     elements = frozenset(items)
     obj = Basic.__new__(cls, elements)
     obj.elements = elements
     obj._dict = dict(items)  # In case Tuple decides it wants to sympify
     return obj

    def solve_multipliers(self, op_point=None, sol_type='dict'):
        """Solves for the values of the lagrange multipliers symbolically at
        the specified operating point

        Parameters
        ==========
        op_point : dict or iterable of dicts, optional
            Point at which to solve at. The operating point is specified as
            a dictionary or iterable of dictionaries of {symbol: value}. The
            value may be numeric or symbolic itself.

        sol_type : str, optional
            Solution return type. Valid options are:
            - 'dict': A dict of {symbol : value} (default)
            - 'Matrix': An ordered column matrix of the solution
        """

        # Determine number of multipliers
        k = len(self.lam_vec)
        if k == 0:
            raise ValueError("System has no lagrange multipliers to solve for.")
        # Compose dict of operating conditions
        if isinstance(op_point, dict):
            op_point_dict = op_point
        elif iterable(op_point):
            op_point_dict = {}
            for op in op_point:
                op_point_dict.update(op)
        elif op_point is None:
            op_point_dict = {}
        else:
            raise TypeError("op_point must be either a dictionary or an "
                            "iterable of dictionaries.")
        # Compose the system to be solved
        mass_matrix = self.mass_matrix.col_join((-self.lam_coeffs.row_join(
                zeros(k, k))))
        force_matrix = self.forcing.col_join(self._f_cd)
        # Sub in the operating point
        mass_matrix = msubs(mass_matrix, op_point_dict)
        force_matrix = msubs(force_matrix, op_point_dict)
        # Solve for the multipliers
        sol_list = mass_matrix.LUsolve(-force_matrix)[-k:]
        if sol_type == 'dict':
            return dict(zip(self.lam_vec, sol_list))
        elif sol_type == 'Matrix':
            return Matrix(sol_list)
        else:
            raise ValueError("Unknown sol_type {:}.".format(sol_type))

from sympy.utilities.iterables import iterable


def do_once(method):
    "A decorator that runs a method only once."

    attrname = "_%s_result" % id(method)

    def decorated(self, *args, **kwargs):
        try:
            return getattr(self, attrname)
        except AttributeError:
            setattr(self, attrname, method(self, *args, **kwargs))
            return getattr(self, attrname)
    return decorated


iterate = lambda x: iter(x) if iterable(x) else iter([x])

    def __init__(self, Lagrangian, qs, coneqs=None, forcelist=None,
            frame=None, hol_coneqs=None, nonhol_coneqs=None):
        """Supply the following for the initialization of LagrangesMethod

        Lagrangian : Sympifyable

        qs: array_like
            The generalized coordinates

        hol_coneqs: array_like, optional
            The holonomic constraint equations

        nonhol_coneqs: array_like, optional
            The nonholonomic constraint equations

        forcelist : iterable, optional
            Takes an iterable of (Point, Vector) or (ReferenceFrame, Vector)
            tuples which represent the force at a point or torque on a frame.
            This feature is primarily to account for the nonconservative forces
            and/or moments.

        frame : ReferenceFrame, optional
            Supply the inertial frame. This is used to determine the
            generalized forces due to non-conservative forces.
        """

        self._L = Matrix([sympify(Lagrangian)])
        self.eom = None
        self._m_cd = Matrix()           # Mass Matrix of differentiated coneqs
        self._m_d = Matrix()            # Mass Matrix of dynamic equations
        self._f_cd = Matrix()           # Forcing part of the diff coneqs
        self._f_d = Matrix()            # Forcing part of the dynamic equations
        self.lam_coeffs = Matrix()      # The coeffecients of the multipliers

        forcelist = forcelist if forcelist else []
        if not iterable(forcelist):
            raise TypeError('Force pairs must be supplied in an iterable.')
        self._forcelist = forcelist
        if frame and not isinstance(frame, ReferenceFrame):
            raise TypeError('frame must be a valid ReferenceFrame')
        self.inertial = frame

        self.lam_vec = Matrix()

        self._term1 = Matrix()
        self._term2 = Matrix()
        self._term3 = Matrix()
        self._term4 = Matrix()

        # Creating the qs, qdots and qdoubledots
        if not iterable(qs):
            raise TypeError('Generalized coordinates must be an iterable')
        self._q = Matrix(qs)
        self._qdots = self.q.diff(dynamicsymbols._t)
        self._qdoubledots = self._qdots.diff(dynamicsymbols._t)

        # Deal with constraint equations
        if coneqs:
            SymPyDeprecationWarning("The `coneqs` kwarg is deprecated in "
                    "favor of `hol_coneqs` and `nonhol_coneqs`. Please "
                    "update your code").warn()
            self.coneqs = coneqs
        else:
            mat_build = lambda x: Matrix(x) if x else Matrix()
            hol_coneqs = mat_build(hol_coneqs)
            nonhol_coneqs = mat_build(nonhol_coneqs)
            self.coneqs = Matrix([hol_coneqs.diff(dynamicsymbols._t),
                    nonhol_coneqs])
            self._hol_coneqs = hol_coneqs

    def _form_frstar(self, bl):
        """Form the generalized inertia force."""

        if not iterable(bl):
            raise TypeError('Bodies must be supplied in an iterable.')

        t = dynamicsymbols._t
        N = self._inertial
        # Dicts setting things to zero
        udot_zero = dict((i, 0) for i in self._udot)
        uaux_zero = dict((i, 0) for i in self._uaux)
        uauxdot = [diff(i, t) for i in self._uaux]
        uauxdot_zero = dict((i, 0) for i in uauxdot)
        # Dictionary of q' and q'' to u and u'
        q_ddot_u_map = dict((k.diff(t), v.diff(t)) for (k, v) in
                self._qdot_u_map.items())
        q_ddot_u_map.update(self._qdot_u_map)

        # Fill up the list of partials: format is a list with num elements
        # equal to number of entries in body list. Each of these elements is a
        # list - either of length 1 for the translational components of
        # particles or of length 2 for the translational and rotational
        # components of rigid bodies. The inner most list is the list of
        # partial velocities.
        def get_partial_velocity(body):
            if isinstance(body, RigidBody):
                vlist = [body.masscenter.vel(N), body.frame.ang_vel_in(N)]
            elif isinstance(body, Particle):
                vlist = [body.point.vel(N),]
            else:
                raise TypeError('The body list may only contain either '
                                'RigidBody or Particle as list elements.')
            v = [msubs(vel, self._qdot_u_map) for vel in vlist]
            return partial_velocity(v, self.u, N)
        partials = [get_partial_velocity(body) for body in bl]

        # Compute fr_star in two components:
        # fr_star = -(MM*u' + nonMM)
        o = len(self.u)
        MM = zeros(o, o)
        nonMM = zeros(o, 1)
        zero_uaux = lambda expr: msubs(expr, uaux_zero)
        zero_udot_uaux = lambda expr: msubs(msubs(expr, udot_zero), uaux_zero)
        for i, body in enumerate(bl):
            if isinstance(body, RigidBody):
                M = zero_uaux(body.mass)
                I = zero_uaux(body.central_inertia)
                vel = zero_uaux(body.masscenter.vel(N))
                omega = zero_uaux(body.frame.ang_vel_in(N))
                acc = zero_udot_uaux(body.masscenter.acc(N))
                inertial_force = (M.diff(t) * vel + M * acc)
                inertial_torque = zero_uaux((I.dt(body.frame) & omega) +
                    msubs(I & body.frame.ang_acc_in(N), udot_zero) +
                    (omega ^ (I & omega)))
                for j in range(o):
                    tmp_vel = zero_uaux(partials[i][0][j])
                    tmp_ang = zero_uaux(I & partials[i][1][j])
                    for k in range(o):
                        # translational
                        MM[j, k] += M * (tmp_vel & partials[i][0][k])
                        # rotational
                        MM[j, k] += (tmp_ang & partials[i][1][k])
                    nonMM[j] += inertial_force & partials[i][0][j]
                    nonMM[j] += inertial_torque & partials[i][1][j]
            else:
                M = zero_uaux(body.mass)
                vel = zero_uaux(body.point.vel(N))
                acc = zero_udot_uaux(body.point.acc(N))
                inertial_force = (M.diff(t) * vel + M * acc)
                for j in range(o):
                    temp = zero_uaux(partials[i][0][j])
                    for k in range(o):
                        MM[j, k] += M * (temp & partials[i][0][k])
                    nonMM[j] += inertial_force & partials[i][0][j]
        # Compose fr_star out of MM and nonMM
        MM = zero_uaux(msubs(MM, q_ddot_u_map))
        nonMM = msubs(msubs(nonMM, q_ddot_u_map),
                udot_zero, uauxdot_zero, uaux_zero)
        fr_star = -(MM * msubs(Matrix(self._udot), uauxdot_zero) + nonMM)

        # If there are dependent speeds, we need to find fr_star_tilde
        if self._udep:
            p = o - len(self._udep)
            fr_star_ind = fr_star[:p, 0]
            fr_star_dep = fr_star[p:o, 0]
            fr_star = fr_star_ind + (self._Ars.T * fr_star_dep)
            # Apply the same to MM
            MMi = MM[:p, :]
            MMd = MM[p:o, :]
            MM = MMi + (self._Ars.T * MMd)

        self._bodylist = bl
        self._frstar = fr_star
        self._k_d = MM
        self._f_d = -msubs(self._fr + self._frstar, udot_zero)
        return fr_star