def _form_fr(self, fl): """Form the generalized active force.""" if not iterable(fl): raise TypeError('Force pairs must be supplied in an iterable.') N = self._inertial # pull out relevant velocities for constructing partial velocities vel_list, f_list = _f_list_parser(fl, N) vel_list = [i.subs(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._fr = FR return FR
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 form_lagranges_equations(self): """Method to form Lagrange's equations of motion. Returns a vector of equations of motion using Lagrange's equations of the second kind. """ qds = self._qdots qdd_zero = dict((i, 0) for i in self._qdoubledots) n = len(self.q) # Internally we represent the EOM as four terms: # EOM = term1 - term2 - term3 - term4 = 0 # First term self._term1 = self._L.jacobian(qds) self._term1 = self._term1.diff(dynamicsymbols._t).T # Second term self._term2 = self._L.jacobian(self.q).T # Third term if self.coneqs: coneqs = self.coneqs m = len(coneqs) # Creating the multipliers self.lam_vec = Matrix(dynamicsymbols('lam1:' + str(m + 1))) self.lam_coeffs = -coneqs.jacobian(qds) self._term3 = self.lam_coeffs.T * self.lam_vec # Extracting the coeffecients of the qdds from the diff coneqs diffconeqs = coneqs.diff(dynamicsymbols._t) self._m_cd = diffconeqs.jacobian(self._qdoubledots) # The remaining terms i.e. the 'forcing' terms in diff coneqs self._f_cd = -diffconeqs.subs(qdd_zero) else: self._term3 = zeros(n, 1) # Fourth term if self.forcelist: N = self.inertial self._term4 = zeros(n, 1) flist = zip(*_f_list_parser(self.forcelist, N)) for i, qd in enumerate(qds): for obj, force in self.forcelist: self._term4[i] = sum(v.diff(qd, N) & f for (v, f) in flist) else: self._term4 = zeros(n, 1) # Form the dynamic mass and forcing matrices without_lam = self._term1 - self._term2 - self._term4 self._m_d = without_lam.jacobian(self._qdoubledots) self._f_d = -without_lam.subs(qdd_zero) # Form the EOM self.eom = without_lam - self._term3 return self.eom
def form_lagranges_equations(self): """Method to form Lagrange's equations of motion. Returns a vector of equations of motion using Lagrange's equations of the second kind. """ qds = self._qdots qdd_zero = dict((i, 0) for i in self._qdoubledots) n = len(self.q) # Internally we represent the EOM as four terms: # EOM = term1 - term2 - term3 - term4 = 0 # First term self._term1 = self._L.jacobian(qds) self._term1 = self._term1.diff(dynamicsymbols._t).T # Second term self._term2 = self._L.jacobian(self.q).T # Third term if self.coneqs: coneqs = self.coneqs m = len(coneqs) # Creating the multipliers self.lam_vec = Matrix(dynamicsymbols('lam1:' + str(m + 1))) self.lam_coeffs = -coneqs.jacobian(qds) self._term3 = self.lam_coeffs.T * self.lam_vec # Extracting the coeffecients of the qdds from the diff coneqs diffconeqs = coneqs.diff(dynamicsymbols._t) self._m_cd = diffconeqs.jacobian(self._qdoubledots) # The remaining terms i.e. the 'forcing' terms in diff coneqs self._f_cd = -diffconeqs.subs(qdd_zero) else: self._term3 = zeros(n, 1) # Fourth term if self.forcelist: N = self.inertial self._term4 = zeros(n, 1) flist = zip(*_f_list_parser(self.forcelist, N)) for i, qd in enumerate(qds): for obj, force in self.forcelist: self._term4[i] = sum(v.diff(qd, N) & f for (v, f) in flist) else: self._term4 = zeros(n, 1) # Form the dynamic mass and forcing matrices without_lam = self._term1 - self._term2 - self._term4 self._m_d = without_lam.jacobian(self._qdoubledots) self._f_d = -without_lam.subs(qdd_zero) # Form the EOM self.eom = without_lam - self._term3 return self.eom