def linearizeQ(EOM, q, op_point=[], noAcc=True, noVel=False, extraSubs=[]): qd = [qi.diff(dynamicsymbols._t) for qi in q] qdd = [qdi.diff(dynamicsymbols._t) for qdi in qd] with Timer('Linearization', True): # NOTE: order important op_point = [] if noAcc: op_point += [(qddi, 0) for qddi in qdd] if noVel: op_point += [(qdi, 0) for qdi in qd] # --- Inputs are dynamic symbols that are not coordinates dyn_symbols = find_dynamicsymbols(EOM) all_q = q + qd + qdd inputs = [s for s in dyn_symbols if s not in all_q] II = np.argsort([str(v) for v in inputs]) # sorting for consistency inputs = list(np.array(inputs)[II]) # KEEP ME Alternative #M, A, B = linearizer.linearize(op_point=op_point ) #o M = -EOM.jacobian(qdd).subs(extraSubs).subs(op_point) C = -EOM.jacobian(qd).subs(extraSubs).subs(op_point) K = -EOM.jacobian(q).subs(extraSubs).subs(op_point) if len(inputs) > 0: B = EOM.jacobian(inputs).subs(extraSubs).subs(op_point) else: B = Matrix([]) return M, C, K, B, inputs
def cleanPyAtom(atom, dofs=None): symbols = atom.free_symbols try: symbols.remove(Symbol('t')) except: pass dyn_symbols = find_dynamicsymbols(atom) #print('>>> symbols', symbols) #print('>>> Dynsymbols', dyn_symbols) s=repr(atom).replace(' ','') # Replace if replDict is not None: for k,v in replDict.items(): if len(v)==2: if s.find(k)>=0: if v[1] is not None: s=s.replace(k, 'p[\'{}\'][{}]'.format(v[0],','.join([str(ii) for ii in v[1]]) ) ) parameters.append(v[0]) else: s=s.replace(k, 'p[\'{}\']'.format(v[0])) parameters.append(v[0]) else: if s.find(k)>=0: s=s.replace(k,v) if dofs is not None: # time derivatives first!!! important for idof,dof in zip(IDOF,dofs): sdof=repr(dof) #s=s.replace('Derivative({}(t),t)'.format(sdof),'qd[{}]'.format(idof)) s=s.replace('Derivative({},t)'.format(sdof),'qd[{}]'.format(idof)) # then Dof for idof,dof in zip(IDOF,dofs): sdof=repr(dof) s=s.replace(sdof+'(t)','q[{}]'.format(idof)) s=s.replace(sdof,'q[{}]'.format(idof)) for symb in symbols: ssymb=repr(symb) if not any([p.find(ssymb)>0 for p in parameters]): if s.find(ssymb)>=0: s=s.replace(ssymb,'p[\'{}\']'.format(ssymb)) parameters.append(ssymb) else: parametersProblem.append(ssymb) for symb in dyn_symbols: ssymb=repr(symb).replace(' ','') if ssymb not in sdofsDeriv and ssymb not in sdofs: ssymb=ssymb.replace('(t)','') if noTimeDep: s=s.replace(ssymb+'(t)','u[\'{}\']'.format(ssymb)) # When linearizing, the "u" is a u0 else: s=s.replace(ssymb,'u[\'{}\']'.format(ssymb)) inputs.append(ssymb) return s
def output_equation(self, output_equation): if isinstance(output_equation, sp.Expr): output_equation = Array([output_equation]) if output_equation is None and self.dim_state == 0: output_equation = empty_array() else: if output_equation is None: output_equation = self.state assert output_equation.atoms( sp.Symbol) <= (set(self.constants_values.keys()) | set([dynamicsymbols._t])) if self.dim_state: assert find_dynamicsymbols(output_equation) <= set(self.state) else: assert find_dynamicsymbols(output_equation) <= set(self.input) self.dim_output = len(output_equation) self._output_equation = output_equation self.update_output_equation_function()
def _Kane_undefined_dynamicsymbols(self): """Similar to ``_find_dynamicsymbols()``, except that it checks all syms used in the system. Code is copied from ``linearize()``. TODO temporarily here until KanesMethod and Lagranges method have an interface for obtaining these quantities. """ from_eoms, from_sym_lists = self._Kane_inlist_insyms() functions_of_time = set() for expr in from_eoms: functions_of_time = functions_of_time.union( find_dynamicsymbols(expr)) return functions_of_time.difference(from_sym_lists)
def dynamic_symbols(self): """Returns a column matrix containing all of the symbols in the system that depend on time""" # Create a list of all of the expressions in the equations of motion if self._comb_explicit_rhs is None: eom_expressions = self.comb_implicit_mat[:] + self.comb_implicit_rhs[:] else: eom_expressions = self._comb_explicit_rhs[:] functions_of_time = set() for expr in eom_expressions: functions_of_time = functions_of_time.union( find_dynamicsymbols(expr)) functions_of_time = functions_of_time.union(self._states) return tuple(functions_of_time)
def dynamic_symbols(self): """Returns a column matrix containing all of the symbols in the system that depend on time""" # Create a list of all of the expressions in the equations of motion if self._comb_explicit_rhs is None: eom_expressions = (self.comb_implicit_mat[:] + self.comb_implicit_rhs[:]) else: eom_expressions = (self._comb_explicit_rhs[:]) functions_of_time = set() for expr in eom_expressions: functions_of_time = functions_of_time.union( find_dynamicsymbols(expr)) functions_of_time = functions_of_time.union(self._states) return tuple(functions_of_time)
def state_equation(self, state_equation): if state_equation is None: # or other checks? state_equation = empty_array() else: assert len(state_equation) == len(self.state) assert find_dynamicsymbols(state_equation) <= (set(self.state) | set(self.input)) assert state_equation.atoms( sp.Symbol) <= (set(self.constants_values.keys()) | set([dynamicsymbols._t])) self._state_equation = state_equation self.update_state_equation_function() self.state_jacobian_equation = grad(self.state_equation, self.state) self.update_state_jacobian_function() self.input_jacobian_equation = grad(self.state_equation, self.input) self.update_input_jacobian_function()
def free_dynamicsymbols(self, reference_frame): """Returns the free dynamic symbols (functions of time ``t``) in the measure numbers of the vector expressed in the given reference frame. Parameters ========== reference_frame : ReferenceFrame The frame with respect to which the free dynamic symbols of the given vector is to be determined. Returns ======= set Set of functions of time ``t``, e.g. ``Function('f')(me.dynamicsymbols._t)``. """ # TODO : Circular dependency if imported at top. Should move # find_dynamicsymbols into physics.vector.functions. from sympy.physics.mechanics.functions import find_dynamicsymbols return find_dynamicsymbols(self, reference_frame=reference_frame)
def _old_linearize(self): """Old method to linearize the equations of motion. Returns a tuple of (f_lin_A, f_lin_B, y) for forming [M]qudot = [f_lin_A]qu + [f_lin_B]y. Deprecated in favor of new method using Linearizer class. Please change your code to use the new `linearize` method.""" if (self._fr is None) or (self._frstar is None): raise ValueError('Need to compute Fr, Fr* first.') # Note that this is now unneccessary, and it should never be # encountered; I still think it should be in here in case the user # manually sets these matrices incorrectly. for i in self._q: if self._k_kqdot.diff(i) != 0 * self._k_kqdot: raise ValueError('Matrix K_kqdot must not depend on any q.') t = dynamicsymbols._t uaux = self._uaux uauxdot = [diff(i, t) for i in uaux] # dictionary of auxiliary speeds & derivatives which are equal to zero subdict = dict( list(zip(uaux + uauxdot, [0] * (len(uaux) + len(uauxdot))))) # Checking for dynamic symbols outside the dynamic differential # equations; throws error if there is. insyms = set( Matrix([self._q, self._qdot, self._u, self._udot, uaux, uauxdot])) if any( find_dynamicsymbols(i, insyms) for i in [ self._k_kqdot, self._k_ku, self._f_k, self._k_dnh, self._f_dnh, self._k_d ]): raise ValueError('Cannot have dynamicsymbols outside dynamic \ forcing vector.') other_dyns = list(find_dynamicsymbols(self._f_d.subs(subdict), insyms)) # make it canonically ordered so the jacobian is canonical other_dyns.sort(key=default_sort_key) for i in other_dyns: if diff(i, dynamicsymbols._t) in other_dyns: raise ValueError('Cannot have derivatives of specified ' 'quantities when linearizing forcing terms.') o = len(self._u) # number of speeds n = len(self._q) # number of coordinates l = len(self._qdep) # number of configuration constraints m = len(self._udep) # number of motion constraints qi = Matrix(self._q[:n - l]) # independent coords qd = Matrix(self._q[n - l:n]) # dependent coords; could be empty ui = Matrix(self._u[:o - m]) # independent speeds ud = Matrix(self._u[o - m:o]) # dependent speeds; could be empty qdot = Matrix(self._qdot) # time derivatives of coordinates # with equations in the form MM udot = forcing, expand that to: # MM_full [q,u].T = forcing_full. This combines coordinates and # speeds together for the linearization, which is necessary for the # linearization process, due to dependent coordinates. f1 is the rows # from the kinematic differential equations, f2 is the rows from the # dynamic differential equations (and differentiated non-holonomic # constraints). f1 = self._k_ku * Matrix(self._u) + self._f_k f2 = self._f_d # Only want to do this if these matrices have been filled in, which # occurs when there are dependent speeds if m != 0: f2 = self._f_d.col_join(self._f_dnh) fnh = self._f_nh + self._k_nh * Matrix(self._u) f1 = f1.subs(subdict) f2 = f2.subs(subdict) fh = self._f_h.subs(subdict) fku = (self._k_ku * Matrix(self._u)).subs(subdict) fkf = self._f_k.subs(subdict) # In the code below, we are applying the chain rule by hand on these # things. All the matrices have been changed into vectors (by # multiplying the dynamic symbols which it is paired with), so we can # take the jacobian of them. The basic operation is take the jacobian # of the f1, f2 vectors wrt all of the q's and u's. f1 is a function of # q, u, and t; f2 is a function of q, qdot, u, and t. In the code # below, we are not considering perturbations in t. So if f1 is a # function of the q's, u's but some of the q's or u's could be # dependent on other q's or u's (qd's might be dependent on qi's, ud's # might be dependent on ui's or qi's), so what we do is take the # jacobian of the f1 term wrt qi's and qd's, the jacobian wrt the qd's # gets multiplied by the jacobian of qd wrt qi, this is extended for # the ud's as well. dqd_dqi is computed by taking a taylor expansion of # the holonomic constraint equations about q*, treating q* - q as dq, # separating into dqd (depedent q's) and dqi (independent q's) and the # rearranging for dqd/dqi. This is again extended for the speeds. # First case: configuration and motion constraints if (l != 0) and (m != 0): fh_jac_qi = fh.jacobian(qi) fh_jac_qd = fh.jacobian(qd) fnh_jac_qi = fnh.jacobian(qi) fnh_jac_qd = fnh.jacobian(qd) fnh_jac_ui = fnh.jacobian(ui) fnh_jac_ud = fnh.jacobian(ud) fku_jac_qi = fku.jacobian(qi) fku_jac_qd = fku.jacobian(qd) fku_jac_ui = fku.jacobian(ui) fku_jac_ud = fku.jacobian(ud) fkf_jac_qi = fkf.jacobian(qi) fkf_jac_qd = fkf.jacobian(qd) f1_jac_qi = f1.jacobian(qi) f1_jac_qd = f1.jacobian(qd) f1_jac_ui = f1.jacobian(ui) f1_jac_ud = f1.jacobian(ud) f2_jac_qi = f2.jacobian(qi) f2_jac_qd = f2.jacobian(qd) f2_jac_ui = f2.jacobian(ui) f2_jac_ud = f2.jacobian(ud) f2_jac_qdot = f2.jacobian(qdot) dqd_dqi = -fh_jac_qd.LUsolve(fh_jac_qi) dud_dqi = fnh_jac_ud.LUsolve(fnh_jac_qd * dqd_dqi - fnh_jac_qi) dud_dui = -fnh_jac_ud.LUsolve(fnh_jac_ui) dqdot_dui = -self._k_kqdot.inv() * (fku_jac_ui + fku_jac_ud * dud_dui) dqdot_dqi = -self._k_kqdot.inv() * ( fku_jac_qi + fkf_jac_qi + (fku_jac_qd + fkf_jac_qd) * dqd_dqi + fku_jac_ud * dud_dqi) f1_q = f1_jac_qi + f1_jac_qd * dqd_dqi + f1_jac_ud * dud_dqi f1_u = f1_jac_ui + f1_jac_ud * dud_dui f2_q = (f2_jac_qi + f2_jac_qd * dqd_dqi + f2_jac_qdot * dqdot_dqi + f2_jac_ud * dud_dqi) f2_u = f2_jac_ui + f2_jac_ud * dud_dui + f2_jac_qdot * dqdot_dui # Second case: configuration constraints only elif l != 0: dqd_dqi = -fh.jacobian(qd).LUsolve(fh.jacobian(qi)) dqdot_dui = -self._k_kqdot.inv() * fku.jacobian(ui) dqdot_dqi = -self._k_kqdot.inv() * ( fku.jacobian(qi) + fkf.jacobian(qi) + (fku.jacobian(qd) + fkf.jacobian(qd)) * dqd_dqi) f1_q = (f1.jacobian(qi) + f1.jacobian(qd) * dqd_dqi) f1_u = f1.jacobian(ui) f2_jac_qdot = f2.jacobian(qdot) f2_q = (f2.jacobian(qi) + f2.jacobian(qd) * dqd_dqi + f2.jac_qdot * dqdot_dqi) f2_u = f2.jacobian(ui) + f2_jac_qdot * dqdot_dui # Third case: motion constraints only elif m != 0: dud_dqi = fnh.jacobian(ud).LUsolve(-fnh.jacobian(qi)) dud_dui = -fnh.jacobian(ud).LUsolve(fnh.jacobian(ui)) dqdot_dui = -self._k_kqdot.inv() * (fku.jacobian(ui) + fku.jacobian(ud) * dud_dui) dqdot_dqi = -self._k_kqdot.inv() * (fku.jacobian(qi) + fkf.jacobian(qi) + fku.jacobian(ud) * dud_dqi) f1_jac_ud = f1.jacobian(ud) f2_jac_qdot = f2.jacobian(qdot) f2_jac_ud = f2.jacobian(ud) f1_q = f1.jacobian(qi) + f1_jac_ud * dud_dqi f1_u = f1.jacobian(ui) + f1_jac_ud * dud_dui f2_q = (f2.jacobian(qi) + f2_jac_qdot * dqdot_dqi + f2_jac_ud * dud_dqi) f2_u = (f2.jacobian(ui) + f2_jac_ud * dud_dui + f2_jac_qdot * dqdot_dui) # Fourth case: No constraints else: dqdot_dui = -self._k_kqdot.inv() * fku.jacobian(ui) dqdot_dqi = -self._k_kqdot.inv() * (fku.jacobian(qi) + fkf.jacobian(qi)) f1_q = f1.jacobian(qi) f1_u = f1.jacobian(ui) f2_jac_qdot = f2.jacobian(qdot) f2_q = f2.jacobian(qi) + f2_jac_qdot * dqdot_dqi f2_u = f2.jacobian(ui) + f2_jac_qdot * dqdot_dui f_lin_A = -(f1_q.row_join(f1_u)).col_join(f2_q.row_join(f2_u)) if other_dyns: f1_oths = f1.jacobian(other_dyns) f2_oths = f2.jacobian(other_dyns) f_lin_B = -f1_oths.col_join(f2_oths) else: f_lin_B = Matrix() return (f_lin_A, f_lin_B, Matrix(other_dyns))
def to_linearizer(self): """Returns an instance of the Linearizer class, initiated from the data in the KanesMethod class. This may be more desirable than using the linearize class method, as the Linearizer object will allow more efficient recalculation (i.e. about varying operating points).""" if (self._fr is None) or (self._frstar is None): raise ValueError('Need to compute Fr, Fr* first.') # Get required equation components. The Kane's method class breaks # these into pieces. Need to reassemble f_c = self._f_h if self._f_nh and self._k_nh: f_v = self._f_nh + self._k_nh * Matrix(self._u) else: f_v = Matrix() if self._f_dnh and self._k_dnh: f_a = self._f_dnh + self._k_dnh * Matrix(self._udot) else: f_a = Matrix() # Dicts to sub to zero, for splitting up expressions u_zero = dict((i, 0) for i in self._u) ud_zero = dict((i, 0) for i in self._udot) qd_zero = dict((i, 0) for i in self._qdot) qd_u_zero = dict((i, 0) for i in Matrix([self._qdot, self._u])) # Break the kinematic differential eqs apart into f_0 and f_1 f_0 = msubs(self._f_k, u_zero) + self._k_kqdot * Matrix(self._qdot) f_1 = msubs(self._f_k, qd_zero) + self._k_ku * Matrix(self._u) # Break the dynamic differential eqs into f_2 and f_3 f_2 = msubs(self._frstar, qd_u_zero) f_3 = msubs(self._frstar, ud_zero) + self._fr f_4 = zeros(len(f_2), 1) # Get the required vector components q = self._q u = self._u if self._qdep: q_i = q[:-len(self._qdep)] else: q_i = q q_d = self._qdep if self._udep: u_i = u[:-len(self._udep)] else: u_i = u u_d = self._udep # Form dictionary to set auxiliary speeds & their derivatives to 0. uaux = self._uaux uauxdot = uaux.diff(dynamicsymbols._t) uaux_zero = dict((i, 0) for i in Matrix([uaux, uauxdot])) # Checking for dynamic symbols outside the dynamic differential # equations; throws error if there is. sym_list = set(Matrix([q, self._qdot, u, self._udot, uaux, uauxdot])) if any( find_dynamicsymbols(i, sym_list) for i in [ self._k_kqdot, self._k_ku, self._f_k, self._k_dnh, self._f_dnh, self._k_d ]): raise ValueError('Cannot have dynamicsymbols outside dynamic \ forcing vector.') # Find all other dynamic symbols, forming the forcing vector r. # Sort r to make it canonical. r = list(find_dynamicsymbols(self._f_d.subs(uaux_zero), sym_list)) r.sort(key=default_sort_key) # Check for any derivatives of variables in r that are also found in r. for i in r: if diff(i, dynamicsymbols._t) in r: raise ValueError('Cannot have derivatives of specified \ quantities when linearizing forcing terms.') return Linearizer(f_0, f_1, f_2, f_3, f_4, f_c, f_v, f_a, q, u, q_i, q_d, u_i, u_d, r)
def to_linearizer(self, q_ind=None, qd_ind=None, q_dep=None, qd_dep=None): """Returns an instance of the Linearizer class, initiated from the data in the LagrangesMethod class. This may be more desirable than using the linearize class method, as the Linearizer object will allow more efficient recalculation (i.e. about varying operating points). Parameters ========== q_ind, qd_ind : array_like, optional The independent generalized coordinates and speeds. q_dep, qd_dep : array_like, optional The dependent generalized coordinates and speeds. """ # Compose vectors t = dynamicsymbols._t q = self.q u = self._qdots ud = u.diff(t) # Get vector of lagrange multipliers lams = self.lam_vec mat_build = lambda x: Matrix(x) if x else Matrix() q_i = mat_build(q_ind) q_d = mat_build(q_dep) u_i = mat_build(qd_ind) u_d = mat_build(qd_dep) # Compose general form equations f_c = self._hol_coneqs f_v = self.coneqs f_a = f_v.diff(t) f_0 = u f_1 = -u f_2 = self._term1 f_3 = -(self._term2 + self._term4) f_4 = -self._term3 # Check that there are an appropriate number of independent and # dependent coordinates if len(q_d) != len(f_c) or len(u_d) != len(f_v): raise ValueError( ("Must supply {:} dependent coordinates, and " + "{:} dependent speeds").format(len(f_c), len(f_v))) if set(Matrix([q_i, q_d])) != set(q): raise ValueError("Must partition q into q_ind and q_dep, with " + "no extra or missing symbols.") if set(Matrix([u_i, u_d])) != set(u): raise ValueError("Must partition qd into qd_ind and qd_dep, " + "with no extra or missing symbols.") # Find all other dynamic symbols, forming the forcing vector r. # Sort r to make it canonical. insyms = set(Matrix([q, u, ud, lams])) r = list(find_dynamicsymbols(f_3, insyms)) r.sort(key=default_sort_key) # Check for any derivatives of variables in r that are also found in r. for i in r: if diff(i, dynamicsymbols._t) in r: raise ValueError('Cannot have derivatives of specified \ quantities when linearizing forcing terms.') return Linearizer(f_0, f_1, f_2, f_3, f_4, f_c, f_v, f_a, q, u, q_i, q_d, u_i, u_d, r, lams)
def to_linearizer(self, q_ind=None, qd_ind=None, q_dep=None, qd_dep=None): """Returns an instance of the Linearizer class, initiated from the data in the LagrangesMethod class. This may be more desirable than using the linearize class method, as the Linearizer object will allow more efficient recalculation (i.e. about varying operating points). Parameters ========== q_ind, qd_ind : array_like, optional The independent generalized coordinates and speeds. q_dep, qd_dep : array_like, optional The dependent generalized coordinates and speeds. """ # Compose vectors t = dynamicsymbols._t q = self.q u = self._qdots ud = u.diff(t) # Get vector of lagrange multipliers lams = self.lam_vec mat_build = lambda x: Matrix(x) if x else Matrix() q_i = mat_build(q_ind) q_d = mat_build(q_dep) u_i = mat_build(qd_ind) u_d = mat_build(qd_dep) # Compose general form equations f_c = self._hol_coneqs f_v = self.coneqs f_a = f_v.diff(t) f_0 = u f_1 = -u f_2 = self._term1 f_3 = -(self._term2 + self._term4) f_4 = -self._term3 # Check that there are an appropriate number of independent and # dependent coordinates if len(q_d) != len(f_c) or len(u_d) != len(f_v): raise ValueError(("Must supply {:} dependent coordinates, and " + "{:} dependent speeds").format(len(f_c), len(f_v))) if set(Matrix([q_i, q_d])) != set(q): raise ValueError("Must partition q into q_ind and q_dep, with " + "no extra or missing symbols.") if set(Matrix([u_i, u_d])) != set(u): raise ValueError("Must partition qd into qd_ind and qd_dep, " + "with no extra or missing symbols.") # Find all other dynamic symbols, forming the forcing vector r. # Sort r to make it canonical. insyms = set(Matrix([q, u, ud, lams])) r = list(find_dynamicsymbols(f_3, insyms)) r.sort(key=default_sort_key) # Check for any derivatives of variables in r that are also found in r. for i in r: if diff(i, dynamicsymbols._t) in r: raise ValueError('Cannot have derivatives of specified \ quantities when linearizing forcing terms.') return Linearizer(f_0, f_1, f_2, f_3, f_4, f_c, f_v, f_a, q, u, q_i, q_d, u_i, u_d, r, lams)
def to_linearizer(self): """Returns an instance of the Linearizer class, initiated from the data in the KanesMethod class. This may be more desirable than using the linearize class method, as the Linearizer object will allow more efficient recalculation (i.e. about varying operating points).""" if (self._fr is None) or (self._frstar is None): raise ValueError('Need to compute Fr, Fr* first.') # Get required equation components. The Kane's method class breaks # these into pieces. Need to reassemble f_c = self._f_h if self._f_nh and self._k_nh: f_v = self._f_nh + self._k_nh*Matrix(self.u) else: f_v = Matrix() if self._f_dnh and self._k_dnh: f_a = self._f_dnh + self._k_dnh*Matrix(self._udot) else: f_a = Matrix() # Dicts to sub to zero, for splitting up expressions u_zero = dict((i, 0) for i in self.u) ud_zero = dict((i, 0) for i in self._udot) qd_zero = dict((i, 0) for i in self._qdot) qd_u_zero = dict((i, 0) for i in Matrix([self._qdot, self.u])) # Break the kinematic differential eqs apart into f_0 and f_1 f_0 = msubs(self._f_k, u_zero) + self._k_kqdot*Matrix(self._qdot) f_1 = msubs(self._f_k, qd_zero) + self._k_ku*Matrix(self.u) # Break the dynamic differential eqs into f_2 and f_3 f_2 = msubs(self._frstar, qd_u_zero) f_3 = msubs(self._frstar, ud_zero) + self._fr f_4 = zeros(len(f_2), 1) # Get the required vector components q = self.q u = self.u if self._qdep: q_i = q[:-len(self._qdep)] else: q_i = q q_d = self._qdep if self._udep: u_i = u[:-len(self._udep)] else: u_i = u u_d = self._udep # Form dictionary to set auxiliary speeds & their derivatives to 0. uaux = self._uaux uauxdot = uaux.diff(dynamicsymbols._t) uaux_zero = dict((i, 0) for i in Matrix([uaux, uauxdot])) # Checking for dynamic symbols outside the dynamic differential # equations; throws error if there is. sym_list = set(Matrix([q, self._qdot, u, self._udot, uaux, uauxdot])) if any(find_dynamicsymbols(i, sym_list) for i in [self._k_kqdot, self._k_ku, self._f_k, self._k_dnh, self._f_dnh, self._k_d]): raise ValueError('Cannot have dynamicsymbols outside dynamic \ forcing vector.') # Find all other dynamic symbols, forming the forcing vector r. # Sort r to make it canonical. r = list(find_dynamicsymbols(msubs(self._f_d, uaux_zero), sym_list)) r.sort(key=default_sort_key) # Check for any derivatives of variables in r that are also found in r. for i in r: if diff(i, dynamicsymbols._t) in r: raise ValueError('Cannot have derivatives of specified \ quantities when linearizing forcing terms.') return Linearizer(f_0, f_1, f_2, f_3, f_4, f_c, f_v, f_a, q, u, q_i, q_d, u_i, u_d, r)
def _old_linearize(self): """Old method to linearize the equations of motion. Returns a tuple of (f_lin_A, f_lin_B, y) for forming [M]qudot = [f_lin_A]qu + [f_lin_B]y. Deprecated in favor of new method using Linearizer class. Please change your code to use the new `linearize` method.""" if (self._fr is None) or (self._frstar is None): raise ValueError('Need to compute Fr, Fr* first.') # Note that this is now unneccessary, and it should never be # encountered; I still think it should be in here in case the user # manually sets these matrices incorrectly. for i in self.q: if self._k_kqdot.diff(i) != 0 * self._k_kqdot: raise ValueError('Matrix K_kqdot must not depend on any q.') t = dynamicsymbols._t uaux = self._uaux uauxdot = [diff(i, t) for i in uaux] # dictionary of auxiliary speeds & derivatives which are equal to zero subdict = dict(list(zip(uaux + uauxdot, [0] * (len(uaux) + len(uauxdot))))) # Checking for dynamic symbols outside the dynamic differential # equations; throws error if there is. insyms = set(Matrix([self.q, self._qdot, self.u, self._udot, uaux, uauxdot])) if any(find_dynamicsymbols(i, insyms) for i in [self._k_kqdot, self._k_ku, self._f_k, self._k_dnh, self._f_dnh, self._k_d]): raise ValueError('Cannot have dynamicsymbols outside dynamic \ forcing vector.') other_dyns = list(find_dynamicsymbols(self._f_d.subs(subdict), insyms)) # make it canonically ordered so the jacobian is canonical other_dyns.sort(key=default_sort_key) for i in other_dyns: if diff(i, dynamicsymbols._t) in other_dyns: raise ValueError('Cannot have derivatives of specified ' 'quantities when linearizing forcing terms.') o = len(self.u) # number of speeds n = len(self.q) # number of coordinates l = len(self._qdep) # number of configuration constraints m = len(self._udep) # number of motion constraints qi = Matrix(self.q[: n - l]) # independent coords qd = Matrix(self.q[n - l: n]) # dependent coords; could be empty ui = Matrix(self.u[: o - m]) # independent speeds ud = Matrix(self.u[o - m: o]) # dependent speeds; could be empty qdot = Matrix(self._qdot) # time derivatives of coordinates # with equations in the form MM udot = forcing, expand that to: # MM_full [q,u].T = forcing_full. This combines coordinates and # speeds together for the linearization, which is necessary for the # linearization process, due to dependent coordinates. f1 is the rows # from the kinematic differential equations, f2 is the rows from the # dynamic differential equations (and differentiated non-holonomic # constraints). f1 = self._k_ku * Matrix(self.u) + self._f_k f2 = self._f_d # Only want to do this if these matrices have been filled in, which # occurs when there are dependent speeds if m != 0: f2 = self._f_d.col_join(self._f_dnh) fnh = self._f_nh + self._k_nh * Matrix(self.u) f1 = f1.subs(subdict) f2 = f2.subs(subdict) fh = self._f_h.subs(subdict) fku = (self._k_ku * Matrix(self.u)).subs(subdict) fkf = self._f_k.subs(subdict) # In the code below, we are applying the chain rule by hand on these # things. All the matrices have been changed into vectors (by # multiplying the dynamic symbols which it is paired with), so we can # take the jacobian of them. The basic operation is take the jacobian # of the f1, f2 vectors wrt all of the q's and u's. f1 is a function of # q, u, and t; f2 is a function of q, qdot, u, and t. In the code # below, we are not considering perturbations in t. So if f1 is a # function of the q's, u's but some of the q's or u's could be # dependent on other q's or u's (qd's might be dependent on qi's, ud's # might be dependent on ui's or qi's), so what we do is take the # jacobian of the f1 term wrt qi's and qd's, the jacobian wrt the qd's # gets multiplied by the jacobian of qd wrt qi, this is extended for # the ud's as well. dqd_dqi is computed by taking a taylor expansion of # the holonomic constraint equations about q*, treating q* - q as dq, # separating into dqd (depedent q's) and dqi (independent q's) and the # rearranging for dqd/dqi. This is again extended for the speeds. # First case: configuration and motion constraints if (l != 0) and (m != 0): fh_jac_qi = fh.jacobian(qi) fh_jac_qd = fh.jacobian(qd) fnh_jac_qi = fnh.jacobian(qi) fnh_jac_qd = fnh.jacobian(qd) fnh_jac_ui = fnh.jacobian(ui) fnh_jac_ud = fnh.jacobian(ud) fku_jac_qi = fku.jacobian(qi) fku_jac_qd = fku.jacobian(qd) fku_jac_ui = fku.jacobian(ui) fku_jac_ud = fku.jacobian(ud) fkf_jac_qi = fkf.jacobian(qi) fkf_jac_qd = fkf.jacobian(qd) f1_jac_qi = f1.jacobian(qi) f1_jac_qd = f1.jacobian(qd) f1_jac_ui = f1.jacobian(ui) f1_jac_ud = f1.jacobian(ud) f2_jac_qi = f2.jacobian(qi) f2_jac_qd = f2.jacobian(qd) f2_jac_ui = f2.jacobian(ui) f2_jac_ud = f2.jacobian(ud) f2_jac_qdot = f2.jacobian(qdot) dqd_dqi = - fh_jac_qd.LUsolve(fh_jac_qi) dud_dqi = fnh_jac_ud.LUsolve(fnh_jac_qd * dqd_dqi - fnh_jac_qi) dud_dui = - fnh_jac_ud.LUsolve(fnh_jac_ui) dqdot_dui = - self._k_kqdot.inv() * (fku_jac_ui + fku_jac_ud * dud_dui) dqdot_dqi = - self._k_kqdot.inv() * (fku_jac_qi + fkf_jac_qi + (fku_jac_qd + fkf_jac_qd) * dqd_dqi + fku_jac_ud * dud_dqi) f1_q = f1_jac_qi + f1_jac_qd * dqd_dqi + f1_jac_ud * dud_dqi f1_u = f1_jac_ui + f1_jac_ud * dud_dui f2_q = (f2_jac_qi + f2_jac_qd * dqd_dqi + f2_jac_qdot * dqdot_dqi + f2_jac_ud * dud_dqi) f2_u = f2_jac_ui + f2_jac_ud * dud_dui + f2_jac_qdot * dqdot_dui # Second case: configuration constraints only elif l != 0: dqd_dqi = - fh.jacobian(qd).LUsolve(fh.jacobian(qi)) dqdot_dui = - self._k_kqdot.inv() * fku.jacobian(ui) dqdot_dqi = - self._k_kqdot.inv() * (fku.jacobian(qi) + fkf.jacobian(qi) + (fku.jacobian(qd) + fkf.jacobian(qd)) * dqd_dqi) f1_q = (f1.jacobian(qi) + f1.jacobian(qd) * dqd_dqi) f1_u = f1.jacobian(ui) f2_jac_qdot = f2.jacobian(qdot) f2_q = (f2.jacobian(qi) + f2.jacobian(qd) * dqd_dqi + f2.jac_qdot * dqdot_dqi) f2_u = f2.jacobian(ui) + f2_jac_qdot * dqdot_dui # Third case: motion constraints only elif m != 0: dud_dqi = fnh.jacobian(ud).LUsolve(- fnh.jacobian(qi)) dud_dui = - fnh.jacobian(ud).LUsolve(fnh.jacobian(ui)) dqdot_dui = - self._k_kqdot.inv() * (fku.jacobian(ui) + fku.jacobian(ud) * dud_dui) dqdot_dqi = - self._k_kqdot.inv() * (fku.jacobian(qi) + fkf.jacobian(qi) + fku.jacobian(ud) * dud_dqi) f1_jac_ud = f1.jacobian(ud) f2_jac_qdot = f2.jacobian(qdot) f2_jac_ud = f2.jacobian(ud) f1_q = f1.jacobian(qi) + f1_jac_ud * dud_dqi f1_u = f1.jacobian(ui) + f1_jac_ud * dud_dui f2_q = (f2.jacobian(qi) + f2_jac_qdot * dqdot_dqi + f2_jac_ud * dud_dqi) f2_u = (f2.jacobian(ui) + f2_jac_ud * dud_dui + f2_jac_qdot * dqdot_dui) # Fourth case: No constraints else: dqdot_dui = - self._k_kqdot.inv() * fku.jacobian(ui) dqdot_dqi = - self._k_kqdot.inv() * (fku.jacobian(qi) + fkf.jacobian(qi)) f1_q = f1.jacobian(qi) f1_u = f1.jacobian(ui) f2_jac_qdot = f2.jacobian(qdot) f2_q = f2.jacobian(qi) + f2_jac_qdot * dqdot_dqi f2_u = f2.jacobian(ui) + f2_jac_qdot * dqdot_dui f_lin_A = -(f1_q.row_join(f1_u)).col_join(f2_q.row_join(f2_u)) if other_dyns: f1_oths = f1.jacobian(other_dyns) f2_oths = f2.jacobian(other_dyns) f_lin_B = -f1_oths.col_join(f2_oths) else: f_lin_B = Matrix() return (f_lin_A, f_lin_B, Matrix(other_dyns))
def _initialize_kindiffeq_matrices(self, kdeqs): """Initialize the kinematic differential equation matrices. Parameters ========== kdeqs : sequence of sympy expressions Kinematic differential equations in the form of f(u,q',q,t) where f() = 0. The equations have to be linear in the generalized coordinates and generalized speeds. """ if kdeqs: if len(self.q) != len(kdeqs): raise ValueError('There must be an equal number of kinematic ' 'differential equations and coordinates.') u = self.u qdot = self._qdot kdeqs = Matrix(kdeqs) u_zero = {ui: 0 for ui in u} uaux_zero = {uai: 0 for uai in self._uaux} qdot_zero = {qdi: 0 for qdi in qdot} # Extract the linear coefficient matrices as per the following # equation: # # k_ku(q,t)*u(t) + k_kqdot(q,t)*q'(t) + f_k(q,t) = 0 # k_ku = kdeqs.jacobian(u) k_kqdot = kdeqs.jacobian(qdot) f_k = kdeqs.xreplace(u_zero).xreplace(qdot_zero) # The kinematic differential equations should be linear in both q' # and u, so check for u and q' in the components. dy_syms = find_dynamicsymbols(k_ku.row_join(k_kqdot).row_join(f_k)) nonlin_vars = [vari for vari in u[:] + qdot[:] if vari in dy_syms] if nonlin_vars: msg = ('The provided kinematic differential equations are ' 'nonlinear in {}. They must be linear in the ' 'generalized speeds and derivatives of the generalized ' 'coordinates.') raise ValueError(msg.format(nonlin_vars)) # Solve for q'(t) such that the coefficient matrices are now in # this form: # # k_kqdot^-1*k_ku*u(t) + I*q'(t) + k_kqdot^-1*f_k = 0 # # NOTE : Solving the kinematic differential equations here is not # necessary and prevents the equations from being provided in fully # implicit form. f_k = k_kqdot.LUsolve(f_k) k_ku = k_kqdot.LUsolve(k_ku) k_kqdot = eye(len(qdot)) self._qdot_u_map = dict(zip(qdot, -(k_ku * u + f_k))) self._f_k = f_k.xreplace(uaux_zero) self._k_ku = k_ku.xreplace(uaux_zero) self._k_kqdot = k_kqdot else: self._qdot_u_map = None self._f_k = Matrix() self._k_ku = Matrix() self._k_kqdot = Matrix()