def _partial_velocity(self, vlist, ulist, frame): """Returns the list of partial velocities, replacing qdot's in the velocity list if necessary. """ if self._qdot_u_map is None: raise ___KDEqError v = [vel.subs(self._qdot_u_map) for vel in vlist] return partial_velocity(v, ulist, frame)
def _form_fr(self, fl): """Form the generalized active force. Computes the vector of the generalized active force vector. Used to compute E.o.M. in the form Fr + Fr* = 0. Parameters ========== fl : list Takes in a list of (Point, Vector) or (ReferenceFrame, Vector) tuples which represent the force at a point or torque on a frame. """ if not hasattr(fl, '__iter__'): raise TypeError('Force pairs must be supplied in an iterable.') N = self._inertial self._forcelist = fl[:] u = self._u o = len(u) # number of gen. speeds b = len(fl) # number of forces FR = zeros(o, 1) # pull out relevant velocities for constructing partial velocities vel_list = [] f_list = [] for i in fl: if isinstance(i[0], ReferenceFrame): vel_list += [i[0].ang_vel_in(N)] elif isinstance(i[0], Point): vel_list += [i[0].vel(N)] else: raise TypeError('First entry in pair must be point or frame.') f_list += [i[1]] partials = partial_velocity(vel_list, u, N) # Fill Fr with dot product of partial velocities and forces for i in range(o): for j in range(b): FR[i] -= partials[j][i] & f_list[j] # In case there are dependent speeds m = len(self._udep) # number of dependent speeds if m != 0: p = o - m FRtilde = FR[:p, 0] FRold = FR[p:o, 0] FRtilde += self._Ars.T * FRold FR = FRtilde self._fr = FR return FR
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(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(zip(uaux, [0] * len(uaux))) uadz = dict(zip(uauxdot, [0] * len(uauxdot))) 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 += [ partial_velocity( [v.masscenter.vel(N), v.frame.ang_vel_in(N)], u, N) ] elif isinstance(v, Particle): partials += [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() I, P = v.inertia if P != v.masscenter: # redefine I about the center of mass # have I S/O, want I S/S* # I S/O = I S/S* + I S*/O; I S/S* = I S/O - I S*/O f = v.frame d = v.masscenter.pos_from(P) I -= inertia_of_point_mass(M, d, f) I = I.subs(uaz).doit() for j in range(o): for k in range(o): # translational MM[j, k] += M * (partials[i][0][j].subs(uaz).doit() & partials[i][0][k]) # rotational temp = (I & partials[i][1][j].subs(uaz).doit()) MM[j, k] += (temp & partials[i][1][k]) # translational components nonMM[j] += ( (M.diff(t) * v.masscenter.vel(N)).subs(uaz).doit() & partials[i][0][j]) nonMM[j] += ( M * v.masscenter.acc(N).subs(udotzero).subs(uaz).doit() & partials[i][0][j]) # rotational components omega = v.frame.ang_vel_in(N).subs(uaz).doit() nonMM[j] += ((I.dt(v.frame) & omega).subs(uaz).doit() & partials[i][1][j]) nonMM[j] += ((I & v.frame.ang_acc_in(N) ).subs(udotzero).subs(uaz).doit() & partials[i][1][j]) nonMM[j] += ((omega ^ (I & omega)).subs(uaz).doit() & partials[i][1][j]) if isinstance(v, Particle): M = v.mass.subs(uaz).doit() for j in range(o): for k in range(o): MM[j, k] += M * (partials[i][0][j].subs(uaz).doit() & partials[i][0][k]) nonMM[j] += M.diff(t) * (v.point.vel(N).subs(uaz).doit() & partials[i][0][j]) nonMM[j] += ( M * v.point.acc(N).subs(udotzero).subs(uaz).doit() & partials[i][0][j]) 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): """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(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(zip(uaux, [0] * len(uaux))) uadz = dict(zip(uauxdot, [0] * len(uauxdot))) 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 += [partial_velocity([v.masscenter.vel(N), v.frame.ang_vel_in(N)], u, N)] elif isinstance(v, Particle): partials += [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() I, P = v.inertia if P != v.masscenter: # redefine I about the center of mass # have I S/O, want I S/S* # I S/O = I S/S* + I S*/O; I S/S* = I S/O - I S*/O f = v.frame d = v.masscenter.pos_from(P) I -= inertia_of_point_mass(M, d, f) I = I.subs(uaz).doit() for j in range(o): for k in range(o): # translational MM[j, k] += M * (partials[i][0][j].subs(uaz).doit() & partials[i][0][k]) # rotational temp = (I & partials[i][1][j].subs(uaz).doit()) MM[j, k] += (temp & partials[i][1][k]) # translational components nonMM[j] += ( (M.diff(t) * v.masscenter.vel(N)).subs(uaz).doit() & partials[i][0][j]) nonMM[j] += (M * v.masscenter.acc(N).subs(udotzero).subs(uaz).doit() & partials[i][0][j]) # rotational components omega = v.frame.ang_vel_in(N).subs(uaz).doit() nonMM[j] += ((I.dt(v.frame) & omega).subs(uaz).doit() & partials[i][1][j]) nonMM[j] += ((I & v.frame.ang_acc_in(N)).subs(udotzero).subs(uaz).doit() & partials[i][1][j]) nonMM[j] += ((omega ^ (I & omega)).subs(uaz).doit() & partials[i][1][j]) if isinstance(v, Particle): M = v.mass.subs(uaz).doit() for j in range(o): for k in range(o): MM[j, k] += M * (partials[i][0][j].subs(uaz).doit() & partials[i][0][k]) nonMM[j] += M.diff(t) * (v.point.vel(N).subs(uaz).doit() & partials[i][0][j]) nonMM[j] += (M * v.point.acc(N).subs(udotzero).subs(uaz).doit() & partials[i][0][j]) 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