def set_inertia(self, I): if not isinstance(I[0], Dyadic): raise TypeError("RigidBody inertia must be a Dyadic object.") if not isinstance(I[1], Point): raise TypeError("RigidBody inertia must be about a Point.") self._inertia = I[0] self._inertia_point = I[1] # 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 # I_S/S* = I_S/O - I_S*/O from sympy.physics.mechanics.functions import inertia_of_point_mass I_Ss_O = inertia_of_point_mass(self.mass, self.masscenter.pos_from(I[1]), self.frame) self._central_inertia = I[0] - I_Ss_O
def inertia(self, I): if not isinstance(I[0], Dyadic): raise TypeError("RigidBody inertia must be a Dyadic object.") if not isinstance(I[1], Point): raise TypeError("RigidBody inertia must be about a Point.") self._inertia = I[0] self._inertia_point = I[1] # 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 # I_S/S* = I_S/O - I_S*/O from sympy.physics.mechanics.functions import inertia_of_point_mass I_Ss_O = inertia_of_point_mass(self.mass, self.masscenter.pos_from(I[1]), self.frame) self._central_inertia = I[0] - I_Ss_O
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 isinstance(bl, (list, tuple)): raise TypeError('Bodies must be supplied in a list.') if self._fr == None: raise ValueError('Calculate Fr first, please.') t = dynamicsymbols._t N = self._inertial self._bodylist = bl u = self._u # all speeds udep = self._udep # dependent speeds o = len(u) p = o - len(udep) udot = self._udot udotzero = dict(zip(udot, [0] * len(udot))) 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))) # dictionary of derivatives of auxiliary speeds which are equal to zero uadz = dict(zip(uauxdot, [0] * len(uauxdot))) # Form R*, T* for each body or particle in the list # This is stored as a list of tuples [(r*, t*),...] # Each tuple is for a body or particle # Within each rs is a tuple and ts is a tuple # These have the same structure: ([list], value) # The list is the coefficients of rs/ts wrt udots, value is everything # else in the expression # Partial velocities are stored as a list of tuple; a tuple for each # body # Each tuple has two elements, lists which represent the partial # velocity for each ur; The first list is translational partial # velocities, the second list is rotational translational velocities MM = zeros(o, o) nonMM = zeros(o, 1) rsts = [] partials = [] for i, v in enumerate(bl): # go through list of bodies, particles if isinstance(v, RigidBody): om = v.frame.ang_vel_in(N).subs(uadz).subs(uaz) # ang velocity omp = v.frame.ang_vel_in(N) # ang velocity, for partials alp = v.frame.ang_acc_in(N).subs(uadz).subs(uaz) # ang acc ve = v.mc.vel(N).subs(uadz).subs(uaz) # velocity vep = v.mc.vel(N) # velocity, for partials acc = v.mc.acc(N).subs(uadz).subs(uaz) # acceleration m = (v.mass).subs(uadz).subs(uaz) I, P = v.inertia I = I.subs(uadz).subs(uaz) if P != v.mc: # redefine I about mass center # 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.mc.pos_from(P) I -= inertia_of_point_mass(m, d, f) templist = [] # One could think of r star as a collection of coefficients of # the udots plus another term. What we do here is get all of # these coefficients and store them in a list, then we get the # "other" term and put the list and other term in a tuple, for # each body/particle. The same is done for t star. The reason # for this is to not let the expressions get too large; so we # keep them seperate for as long a possible for j, w in enumerate(udot): templist.append(-m * acc.diff(w, N)) other = -m.diff(t) * ve - m * acc.subs(udotzero) rs = (templist, other) templist = [] # see above note for j, w in enumerate(udot): templist.append(-I & alp.diff(w, N)) other = -((I.dt(v.frame) & om) + (I & alp.subs(udotzero)) + (om ^ (I & om))) ts = (templist, other) tl1 = [] tl2 = [] # calculates the partials only once and stores them for later for j, w in enumerate(u): tl1.append(vep.diff(w, N)) tl2.append(omp.diff(w, N)) partials.append((tl1, tl2)) elif isinstance(v, Particle): ve = v.point.vel(N).subs(uadz).subs(uaz) vep = v.point.vel(N) acc = v.point.acc(N).subs(uadz).subs(uaz) m = v.mass.subs(uadz).subs(uaz) templist = [] # see above note for j, w in enumerate(udot): templist.append(-m * acc.diff(w, N)) other = -m.diff(t) * ve - m * acc.subs(udotzero) rs = (templist, other) # We make an empty t star here so that way the later code # doesn't care whether its operating on a body or particle ts = ([0] * len(u), 0) tl1 = [] tl2 = [] # calculates the partials only once, makes 0's for angular # partials so the later code is body/particle indepedent for j, w in enumerate(u): tl1.append(vep.diff(w, N)) tl2.append(0) partials.append((tl1, tl2)) else: raise TypeError('The body list needs RigidBody or ' 'Particle as list elements.') rsts.append((rs, ts)) # Use R*, T* and partial velocities to form FR* FRSTAR = zeros(o, 1) # does this for each body in the list for i, v in enumerate(rsts): rs, ts = v # unpact r*, t* vps, ops = partials[i] # unpack vel. partials, ang. vel. partials # Computes the mass matrix entries from r*, there are from the list # in the rstar tuple ii = 0 for x in vps: for w in rs[0]: MM[ii] += w & x ii += 1 # Computes the mass matrix entries from t*, there are from the list # in the tstar tuple ii = 0 for x in ops: for w in ts[0]: MM[ii] += w & x ii += 1 # Non mass matrix entries from rstar, from the other in the rstar # tuple for j, w in enumerate(vps): nonMM[j] += w & rs[1] # Non mass matrix entries from tstar, from the other in the tstar # tuple for j, w in enumerate(ops): nonMM[j] += w & ts[1] FRSTAR = MM * Matrix(udot) + 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 len(self._udep) != 0: FRSTARtilde = FRSTAR.extract(range(p), [0]) FRSTARold = FRSTAR.extract(range(p, o), [0]) FRSTARtilde += self._Ars.T * FRSTARold FRSTAR = FRSTARtilde MMi = MM.extract(range(p), range(o)) MMd = MM.extract(range(p, o), range(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
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 isinstance(bl, (list, tuple)): raise TypeError('Bodies must be supplied in a list.') if self._fr == None: raise ValueError('Calculate Fr first, please.') t = dynamicsymbols._t N = self._inertial self._bodylist = bl u = self._u # all speeds udep = self._udep # dependent speeds o = len(u) p = o - len(udep) udot = self._udot udotzero = dict(zip(udot, [0] * len(udot))) 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))) # dictionary of derivatives of auxiliary speeds which are equal to zero uadz = dict(zip(uauxdot, [0] * len(uauxdot))) # Form R*, T* for each body or particle in the list # This is stored as a list of tuples [(r*, t*),...] # Each tuple is for a body or particle # Within each rs is a tuple and ts is a tuple # These have the same structure: ([list], value) # The list is the coefficients of rs/ts wrt udots, value is everything # else in the expression # Partial velocities are stored as a list of tuple; a tuple for each # body # Each tuple has two elements, lists which represent the partial # velocity for each ur; The first list is translational partial # velocities, the second list is rotational translational velocities MM = zeros(o, o) nonMM = zeros(o, 1) rsts = [] partials = [] for i, v in enumerate(bl): # go through list of bodies, particles if isinstance(v, RigidBody): om = v.frame.ang_vel_in(N).subs(uadz).subs(uaz) # ang velocity omp = v.frame.ang_vel_in(N) # ang velocity, for partials alp = v.frame.ang_acc_in(N).subs(uadz).subs(uaz) # ang acc ve = v.masscenter.vel(N).subs(uadz).subs(uaz) # velocity vep = v.masscenter.vel(N) # velocity, for partials acc = v.masscenter.acc(N).subs(uadz).subs(uaz) # acceleration m = (v.mass).subs(uadz).subs(uaz) I, P = v.inertia I = I.subs(uadz).subs(uaz) 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) templist = [] # One could think of r star as a collection of coefficients of # the udots plus another term. What we do here is get all of # these coefficients and store them in a list, then we get the # "other" term and put the list and other term in a tuple, for # each body/particle. The same is done for t star. The reason # for this is to not let the expressions get too large; so we # keep them seperate for as long a possible for j, w in enumerate(udot): templist.append(-m * acc.diff(w, N)) other = -m.diff(t) * ve - m * acc.subs(udotzero) rs = (templist, other) templist = [] # see above note for j, w in enumerate(udot): templist.append(-I & alp.diff(w, N)) other = -((I.dt(v.frame) & om) + (I & alp.subs(udotzero)) + (om ^ (I & om))) ts = (templist, other) tl1 = [] tl2 = [] # calculates the partials only once and stores them for later for j, w in enumerate(u): tl1.append(vep.diff(w, N)) tl2.append(omp.diff(w, N)) partials.append((tl1, tl2)) elif isinstance(v, Particle): ve = v.point.vel(N).subs(uadz).subs(uaz) vep = v.point.vel(N) acc = v.point.acc(N).subs(uadz).subs(uaz) m = v.mass.subs(uadz).subs(uaz) templist = [] # see above note for j, w in enumerate(udot): templist.append(-m * acc.diff(w, N)) other = -m.diff(t) * ve - m * acc.subs(udotzero) rs = (templist, other) # We make an empty t star here so that way the later code # doesn't care whether its operating on a body or particle ts = ([0] * len(u), 0) tl1 = [] tl2 = [] # calculates the partials only once, makes 0's for angular # partials so the later code is body/particle indepedent for j, w in enumerate(u): tl1.append(vep.diff(w, N)) tl2.append(0) partials.append((tl1, tl2)) else: raise TypeError('The body list needs RigidBody or ' 'Particle as list elements.') rsts.append((rs, ts)) # Use R*, T* and partial velocities to form FR* FRSTAR = zeros(o, 1) # does this for each body in the list for i, v in enumerate(rsts): rs, ts = v # unpact r*, t* vps, ops = partials[i] # unpack vel. partials, ang. vel. partials # Computes the mass matrix entries from r*, there are from the list # in the rstar tuple ii = 0 for x in vps: for w in rs[0]: MM[ii] += w & x ii += 1 # Computes the mass matrix entries from t*, there are from the list # in the tstar tuple ii = 0 for x in ops: for w in ts[0]: MM[ii] += w & x ii += 1 # Non mass matrix entries from rstar, from the other in the rstar # tuple for j, w in enumerate(vps): nonMM[j] += w & rs[1] # Non mass matrix entries from tstar, from the other in the tstar # tuple for j, w in enumerate(ops): nonMM[j] += w & ts[1] FRSTAR = MM * Matrix(udot) + 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 len(self._udep) != 0: FRSTARtilde = FRSTAR.extract(range(p), [0]) FRSTARold = FRSTAR.extract(range(p, o), [0]) FRSTARtilde += self._Ars.T * FRSTARold FRSTAR = FRSTARtilde MMi = MM.extract(range(p), range(o)) MMd = MM.extract(range(p, o), range(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