(self, bl)
return FRSTAR
Form the generalized inertia force.

        Computes the vector of the generalized inertia force vector.
        Used to compute E.o.M. in the form Fr + Fr* = 0.

        Parameters
        ==========

        bl : list
            A list of all RigidBody's and Particle's in the system.
Form the generalized inertia force.
Form the generalized inertia force .
def _form_frstar(self, bl):
        """Form the generalized inertia force.

        Computes the vector of the generalized inertia force vector.
        Used to compute E.o.M. in the form Fr + Fr* = 0.

        Parameters
        ==========

        bl : list
            A list of all RigidBody's and Particle's in the system.

        """

        if not hasattr(bl, '__iter__'):
            raise TypeError('Bodies must be supplied in an iterable.')
        t = dynamicsymbols._t
        N = self._inertial
        self._bodylist = bl
        u = self._u  # all speeds
        udep = self._udep  # dependent speeds
        o = len(u)
        m = len(udep)
        p = o - m
        udot = self._udot
        udotzero = dict(list(zip(udot, [0] * o)))
        # auxiliary speeds
        uaux = self._uaux
        uauxdot = [diff(i, t) for i in uaux]
        # dictionary of auxiliary speeds which are equal to zero
        uaz = dict(list(zip(uaux, [0] * len(uaux))))
        uadz = dict(list(zip(uauxdot, [0] * len(uauxdot))))
        # dictionary of qdot's to u's
        qdots = dict(list(zip(list(self._qdot_u_map.keys()),
                         list(self._qdot_u_map.values()))))
        for k, v in list(qdots.items()):
            qdots[k.diff(t)] = v.diff(t)

        MM = zeros(o, o)
        nonMM = zeros(o, 1)
        partials = []
        # Fill up the list of partials: format is a list with no. 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.
        for v in bl:
            if isinstance(v, RigidBody):
                partials += [self._partial_velocity([v.masscenter.vel(N),
                                                     v.frame.ang_vel_in(N)],
                                                    u, N)]
            elif isinstance(v, Particle):
                partials += [self._partial_velocity([v.point.vel(N)], u, N)]
            else:
                raise TypeError('The body list needs RigidBody or '
                                'Particle as list elements.')

        # This section does 2 things - computes the parts of Fr* that are
        # associated with the udots, and the parts that are not associated with
        # the udots. This happens for RigidBody and Particle a little
        # differently, but similar process overall.
        for i, v in enumerate(bl):
            if isinstance(v, RigidBody):
                M = v.mass.subs(uaz).doit()
                vel = v.masscenter.vel(N).subs(uaz).doit()
                acc = v.masscenter.acc(N).subs(udotzero).subs(uaz).doit()
                inertial_force = (M.diff(t) * vel + M * acc)
                omega = v.frame.ang_vel_in(N).subs(uaz).doit()
                I = v.central_inertia.subs(uaz).doit()
                inertial_torque = ((I.dt(v.frame) & omega).subs(uaz).doit() +
                    (I & v.frame.ang_acc_in(N)).subs(udotzero).subs(uaz).doit() +
                    (omega ^ (I & omega)).subs(uaz).doit())
                for j in range(o):
                    tmp_vel = partials[i][0][j].subs(uaz).doit()
                    tmp_ang = (I & partials[i][1][j].subs(uaz).doit())
                    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]

            if isinstance(v, Particle):
                M = v.mass.subs(uaz).doit()
                vel = v.point.vel(N).subs(uaz).doit()
                acc = v.point.acc(N).subs(udotzero).subs(uaz).doit()
                inertial_force = (M.diff(t) * vel + M * acc)
                for j in range(o):
                    temp = partials[i][0][j].subs(uaz).doit()
                    for k in range(o):
                        MM[j, k] += M * (temp & partials[i][0][k])
                    nonMM[j] += inertial_force & partials[i][0][j]
        # Negate FRSTAR since Kane defines the inertia forces/torques
        # to be negative and we didn't do so above.
        MM = MM.subs(qdots).subs(uaz).doit()
        nonMM = nonMM.subs(qdots).subs(udotzero).subs(uadz).subs(uaz).doit()
        FRSTAR = -(MM * Matrix(udot).subs(uadz) + nonMM)

        # For motion constraints, m is the number of constraints
        # Really, one should just look at Kane's book for descriptions of this
        # process
        if m != 0:
            FRSTARtilde = FRSTAR[:p, 0]
            FRSTARold = FRSTAR[p:o, 0]
            FRSTARtilde += self._Ars.T * FRSTARold
            FRSTAR = FRSTARtilde

            MMi = MM[:p, :]
            MMd = MM[p:o, :]
            MM = MMi + self._Ars.T * MMd
        self._frstar = FRSTAR

        zeroeq = -(self._fr + self._frstar)
        zeroeq = zeroeq.subs(udotzero)

        self._k_d = MM
        self._f_d = zeroeq
        return FRSTAR
def _form_frstar ( self , bl ) : if not hasattr ( bl , '__iter__' ) : raise TypeError ( 'Bodies must be supplied in an iterable.' ) t = dynamicsymbols . _t N = self . _inertial self . _bodylist = bl u = self . _u # all speeds udep = self . _udep # dependent speeds o = len ( u ) m = len ( udep ) p = o - m udot = self . _udot udotzero = dict ( list ( zip ( udot , [ 0 ] * o ) ) ) # auxiliary speeds uaux = self . _uaux uauxdot = [ diff ( i , t ) for i in uaux ] # dictionary of auxiliary speeds which are equal to zero uaz = dict ( list ( zip ( uaux , [ 0 ] * len ( uaux ) ) ) ) uadz = dict ( list ( zip ( uauxdot , [ 0 ] * len ( uauxdot ) ) ) ) # dictionary of qdot's to u's qdots = dict ( list ( zip ( list ( self . _qdot_u_map . keys ( ) ) , list ( self . _qdot_u_map . values ( ) ) ) ) ) for k , v in list ( qdots . items ( ) ) : qdots [ k . diff ( t ) ] = v . diff ( t ) MM = zeros ( o , o ) nonMM = zeros ( o , 1 ) partials = [ ] # Fill up the list of partials: format is a list with no. 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. for v in bl : if isinstance ( v , RigidBody ) : partials += [ self . _partial_velocity ( [ v . masscenter . vel ( N ) , v . frame . ang_vel_in ( N ) ] , u , N ) ] elif isinstance ( v , Particle ) : partials += [ self . _partial_velocity ( [ v . point . vel ( N ) ] , u , N ) ] else : raise TypeError ( 'The body list needs RigidBody or ' 'Particle as list elements.' ) # This section does 2 things - computes the parts of Fr* that are # associated with the udots, and the parts that are not associated with # the udots. This happens for RigidBody and Particle a little # differently, but similar process overall. for i , v in enumerate ( bl ) : if isinstance ( v , RigidBody ) : M = v . mass . subs ( uaz ) . doit ( ) vel = v . masscenter . vel ( N ) . subs ( uaz ) . doit ( ) acc = v . masscenter . acc ( N ) . subs ( udotzero ) . subs ( uaz ) . doit ( ) inertial_force = ( M . diff ( t ) * vel + M * acc ) omega = v . frame . ang_vel_in ( N ) . subs ( uaz ) . doit ( ) I = v . central_inertia . subs ( uaz ) . doit ( ) inertial_torque = ( ( I . dt ( v . frame ) & omega ) . subs ( uaz ) . doit ( ) + ( I & v . frame . ang_acc_in ( N ) ) . subs ( udotzero ) . subs ( uaz ) . doit ( ) + ( omega ^ ( I & omega ) ) . subs ( uaz ) . doit ( ) ) for j in range ( o ) : tmp_vel = partials [ i ] [ 0 ] [ j ] . subs ( uaz ) . doit ( ) tmp_ang = ( I & partials [ i ] [ 1 ] [ j ] . subs ( uaz ) . doit ( ) ) 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 ] if isinstance ( v , Particle ) : M = v . mass . subs ( uaz ) . doit ( ) vel = v . point . vel ( N ) . subs ( uaz ) . doit ( ) acc = v . point . acc ( N ) . subs ( udotzero ) . subs ( uaz ) . doit ( ) inertial_force = ( M . diff ( t ) * vel + M * acc ) for j in range ( o ) : temp = partials [ i ] [ 0 ] [ j ] . subs ( uaz ) . doit ( ) for k in range ( o ) : MM [ j , k ] += M * ( temp & partials [ i ] [ 0 ] [ k ] ) nonMM [ j ] += inertial_force & partials [ i ] [ 0 ] [ j ] # Negate FRSTAR since Kane defines the inertia forces/torques # to be negative and we didn't do so above. MM = MM . subs ( qdots ) . subs ( uaz ) . doit ( ) nonMM = nonMM . subs ( qdots ) . subs ( udotzero ) . subs ( uadz ) . subs ( uaz ) . doit ( ) FRSTAR = - ( MM * Matrix ( udot ) . subs ( uadz ) + nonMM ) # For motion constraints, m is the number of constraints # Really, one should just look at Kane's book for descriptions of this # process if m != 0 : FRSTARtilde = FRSTAR [ : p , 0 ] FRSTARold = FRSTAR [ p : o , 0 ] FRSTARtilde += self . _Ars . T * FRSTARold FRSTAR = FRSTARtilde MMi = MM [ : p , : ] MMd = MM [ p : o , : ] MM = MMi + self . _Ars . T * MMd self . _frstar = FRSTAR zeroeq = - ( self . _fr + self . _frstar ) zeroeq = zeroeq . subs ( udotzero ) self . _k_d = MM self . _f_d = zeroeq return FRSTAR