def _form_permutation_matrices(self): """Form the permutation matrices Pq and Pu.""" # Extract dimension variables l, m, n, o, s, k = self._dims # Compute permutation matrices if n != 0: self._Pq = permutation_matrix(self.q, Matrix([self.q_i, self.q_d])) if l > 0: self._Pqi = self._Pq[:, :-l] self._Pqd = self._Pq[:, -l:] else: self._Pqi = self._Pq self._Pqd = Matrix() if o != 0: self._Pu = permutation_matrix(self.u, Matrix([self.u_i, self.u_d])) if m > 0: self._Pui = self._Pu[:, :-m] self._Pud = self._Pu[:, -m:] else: self._Pui = self._Pu self._Pud = Matrix() # Compute combination permutation matrix for computing A and B P_col1 = Matrix([self._Pqi, zeros(o + k, n - l)]) P_col2 = Matrix([zeros(n, o - m), self._Pui, zeros(k, o - m)]) if P_col1: if P_col2: self.perm_mat = P_col1.row_join(P_col2) else: self.perm_mat = P_col1 else: self.perm_mat = P_col2
def _initialize_kindiffeq_matrices(self, kdeqs): """Initialize the kinematic differential equation matrices.""" if kdeqs: if len(self.q) != len(kdeqs): raise ValueError('There must be an equal number of kinematic ' 'differential equations and coordinates.') kdeqs = Matrix(kdeqs) u = self.u qdot = self._qdot # Dictionaries setting things to zero u_zero = dict((i, 0) for i in u) uaux_zero = dict((i, 0) for i in self._uaux) qdot_zero = dict((i, 0) for i in qdot) f_k = msubs(kdeqs, u_zero, qdot_zero) k_ku = (msubs(kdeqs, qdot_zero) - f_k).jacobian(u) k_kqdot = (msubs(kdeqs, u_zero) - f_k).jacobian(qdot) f_k = k_kqdot.LUsolve(f_k) k_ku = k_kqdot.LUsolve(k_ku) k_kqdot = eye(len(qdot)) self._qdot_u_map = solve_linear_system_LU( Matrix([k_kqdot.T, -(k_ku * u + f_k).T]).T, qdot) self._f_k = msubs(f_k, uaux_zero) self._k_ku = msubs(k_ku, 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()
def __init__(self, name, indices=None, latexs=None, variables=None): """ReferenceFrame initialization method. A ReferenceFrame has a set of orthonormal basis vectors, along with orientations relative to other ReferenceFrames and angular velocities relative to other ReferenceFrames. Parameters ========== indices : list (of strings) If custom indices are desired for console, pretty, and LaTeX printing, supply three as a list. The basis vectors can then be accessed with the get_item method. latexs : list (of strings) If custom names are desired for LaTeX printing of each basis vector, supply the names here in a list. Examples ======== >>> from sympy.physics.vector import ReferenceFrame, vlatex >>> N = ReferenceFrame('N') >>> N.x N.x >>> O = ReferenceFrame('O', indices=('1', '2', '3')) >>> O.x O['1'] >>> O['1'] O['1'] >>> P = ReferenceFrame('P', latexs=('A1', 'A2', 'A3')) >>> vlatex(P.x) 'A1' """ if not isinstance(name, string_types): raise TypeError('Need to supply a valid name') # The if statements below are for custom printing of basis-vectors for # each frame. # First case, when custom indices are supplied if indices is not None: if not isinstance(indices, (tuple, list)): raise TypeError('Supply the indices as a list') if len(indices) != 3: raise ValueError('Supply 3 indices') for i in indices: if not isinstance(i, string_types): raise TypeError('Indices must be strings') self.str_vecs = [(name + '[\'' + indices[0] + '\']'), (name + '[\'' + indices[1] + '\']'), (name + '[\'' + indices[2] + '\']')] self.pretty_vecs = [(name.lower() + u"_" + indices[0]), (name.lower() + u"_" + indices[1]), (name.lower() + u"_" + indices[2])] self.latex_vecs = [ (r"\mathbf{\hat{%s}_{%s}}" % (name.lower(), indices[0])), (r"\mathbf{\hat{%s}_{%s}}" % (name.lower(), indices[1])), (r"\mathbf{\hat{%s}_{%s}}" % (name.lower(), indices[2])) ] self.indices = indices # Second case, when no custom indices are supplied else: self.str_vecs = [(name + '.x'), (name + '.y'), (name + '.z')] self.pretty_vecs = [ name.lower() + u"_x", name.lower() + u"_y", name.lower() + u"_z" ] self.latex_vecs = [(r"\mathbf{\hat{%s}_x}" % name.lower()), (r"\mathbf{\hat{%s}_y}" % name.lower()), (r"\mathbf{\hat{%s}_z}" % name.lower())] self.indices = ['x', 'y', 'z'] # Different step, for custom latex basis vectors if latexs is not None: if not isinstance(latexs, (tuple, list)): raise TypeError('Supply the indices as a list') if len(latexs) != 3: raise ValueError('Supply 3 indices') for i in latexs: if not isinstance(i, string_types): raise TypeError('Latex entries must be strings') self.latex_vecs = latexs self.name = name self._var_dict = {} #The _dcm_dict dictionary will only store the dcms of parent-child #relationships. The _dcm_cache dictionary will work as the dcm #cache. self._dcm_dict = {} self._dcm_cache = {} self._ang_vel_dict = {} self._ang_acc_dict = {} self._dlist = [self._dcm_dict, self._ang_vel_dict, self._ang_acc_dict] self._cur = 0 self._x = Vector([(Matrix([1, 0, 0]), self)]) self._y = Vector([(Matrix([0, 1, 0]), self)]) self._z = Vector([(Matrix([0, 0, 1]), self)]) #Associate coordinate symbols wrt this frame if variables is not None: if not isinstance(variables, (tuple, list)): raise TypeError('Supply the variable names as a list/tuple') if len(variables) != 3: raise ValueError('Supply 3 variable names') for i in variables: if not isinstance(i, string_types): raise TypeError('Variable names must be strings') else: variables = [name + '_x', name + '_y', name + '_z'] self.varlist = (CoordinateSym(variables[0], self, 0), \ CoordinateSym(variables[1], self, 1), \ CoordinateSym(variables[2], self, 2)) ReferenceFrame._count += 1 self.index = ReferenceFrame._count
class LagrangesMethod(object): """Lagrange's method object. This object generates the equations of motion in a two step procedure. The first step involves the initialization of LagrangesMethod by supplying the Lagrangian and the generalized coordinates, at the bare minimum. If there are any constraint equations, they can be supplied as keyword arguments. The Lagrange multipliers are automatically generated and are equal in number to the constraint equations. Similarly any non-conservative forces can be supplied in an iterable (as described below and also shown in the example) along with a ReferenceFrame. This is also discussed further in the __init__ method. Attributes ========== q, u : Matrix Matrices of the generalized coordinates and speeds forcelist : iterable Iterable of (Point, vector) or (ReferenceFrame, vector) tuples describing the forces on the system. bodies : iterable Iterable containing the rigid bodies and particles of the system. mass_matrix : Matrix The system's mass matrix forcing : Matrix The system's forcing vector mass_matrix_full : Matrix The "mass matrix" for the qdot's, qdoubledot's, and the lagrange multipliers (lam) forcing_full : Matrix The forcing vector for the qdot's, qdoubledot's and lagrange multipliers (lam) Examples ======== This is a simple example for a one degree of freedom translational spring-mass-damper. In this example, we first need to do the kinematics. This involves creating generalized coordinates and their derivatives. Then we create a point and set its velocity in a frame. >>> from sympy.physics.mechanics import LagrangesMethod, Lagrangian >>> from sympy.physics.mechanics import ReferenceFrame, Particle, Point >>> from sympy.physics.mechanics import dynamicsymbols, kinetic_energy >>> from sympy import symbols >>> q = dynamicsymbols('q') >>> qd = dynamicsymbols('q', 1) >>> m, k, b = symbols('m k b') >>> N = ReferenceFrame('N') >>> P = Point('P') >>> P.set_vel(N, qd * N.x) We need to then prepare the information as required by LagrangesMethod to generate equations of motion. First we create the Particle, which has a point attached to it. Following this the lagrangian is created from the kinetic and potential energies. Then, an iterable of nonconservative forces/torques must be constructed, where each item is a (Point, Vector) or (ReferenceFrame, Vector) tuple, with the Vectors representing the nonconservative forces or torques. >>> Pa = Particle('Pa', P, m) >>> Pa.potential_energy = k * q**2 / 2.0 >>> L = Lagrangian(N, Pa) >>> fl = [(P, -b * qd * N.x)] Finally we can generate the equations of motion. First we create the LagrangesMethod object. To do this one must supply the Lagrangian, and the generalized coordinates. The constraint equations, the forcelist, and the inertial frame may also be provided, if relevant. Next we generate Lagrange's equations of motion, such that: Lagrange's equations of motion = 0. We have the equations of motion at this point. >>> l = LagrangesMethod(L, [q], forcelist = fl, frame = N) >>> print(l.form_lagranges_equations()) Matrix([[b*Derivative(q(t), t) + 1.0*k*q(t) + m*Derivative(q(t), t, t)]]) We can also solve for the states using the 'rhs' method. >>> print(l.rhs()) Matrix([[Derivative(q(t), t)], [(-b*Derivative(q(t), t) - 1.0*k*q(t))/m]]) Please refer to the docstrings on each method for more details. """ def __init__(self, Lagrangian, qs, coneqs=None, forcelist=None, bodies=None, frame=None, hol_coneqs=None, nonhol_coneqs=None): """Supply the following for the initialization of LagrangesMethod Lagrangian : Sympifyable qs : array_like The generalized coordinates hol_coneqs : array_like, optional The holonomic constraint equations nonhol_coneqs : array_like, optional The nonholonomic constraint equations forcelist : iterable, optional Takes an iterable of (Point, Vector) or (ReferenceFrame, Vector) tuples which represent the force at a point or torque on a frame. This feature is primarily to account for the nonconservative forces and/or moments. bodies : iterable, optional Takes an iterable containing the rigid bodies and particles of the system. frame : ReferenceFrame, optional Supply the inertial frame. This is used to determine the generalized forces due to non-conservative forces. """ self._L = Matrix([sympify(Lagrangian)]) self.eom = None self._m_cd = Matrix() # Mass Matrix of differentiated coneqs self._m_d = Matrix() # Mass Matrix of dynamic equations self._f_cd = Matrix() # Forcing part of the diff coneqs self._f_d = Matrix() # Forcing part of the dynamic equations self.lam_coeffs = Matrix() # The coeffecients of the multipliers forcelist = forcelist if forcelist else [] if not iterable(forcelist): raise TypeError('Force pairs must be supplied in an iterable.') self._forcelist = forcelist if frame and not isinstance(frame, ReferenceFrame): raise TypeError('frame must be a valid ReferenceFrame') self._bodies = bodies self.inertial = frame self.lam_vec = Matrix() self._term1 = Matrix() self._term2 = Matrix() self._term3 = Matrix() self._term4 = Matrix() # Creating the qs, qdots and qdoubledots if not iterable(qs): raise TypeError('Generalized coordinates must be an iterable') self._q = Matrix(qs) self._qdots = self.q.diff(dynamicsymbols._t) self._qdoubledots = self._qdots.diff(dynamicsymbols._t) # Deal with constraint equations if coneqs: SymPyDeprecationWarning("The `coneqs` kwarg is deprecated in " "favor of `hol_coneqs` and `nonhol_coneqs`. Please " "update your code").warn() self.coneqs = coneqs else: mat_build = lambda x: Matrix(x) if x else Matrix() hol_coneqs = mat_build(hol_coneqs) nonhol_coneqs = mat_build(nonhol_coneqs) self.coneqs = Matrix([hol_coneqs.diff(dynamicsymbols._t), nonhol_coneqs]) self._hol_coneqs = hol_coneqs 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) for i, qd in enumerate(qds): flist = zip(*_f_list_parser(self.forcelist, N)) 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 @property def mass_matrix(self): """Returns the mass matrix, which is augmented by the Lagrange multipliers, if necessary. If the system is described by 'n' generalized coordinates and there are no constraint equations then an n X n matrix is returned. If there are 'n' generalized coordinates and 'm' constraint equations have been supplied during initialization then an n X (n+m) matrix is returned. The (n + m - 1)th and (n + m)th columns contain the coefficients of the Lagrange multipliers. """ if self.eom is None: raise ValueError('Need to compute the equations of motion first') if self.coneqs: return (self._m_d).row_join(self.lam_coeffs.T) else: return self._m_d @property def mass_matrix_full(self): """Augments the coefficients of qdots to the mass_matrix.""" if self.eom is None: raise ValueError('Need to compute the equations of motion first') n = len(self.q) m = len(self.coneqs) row1 = eye(n).row_join(zeros(n, n + m)) row2 = zeros(n, n).row_join(self.mass_matrix) if self.coneqs: row3 = zeros(m, n).row_join(self._m_cd).row_join(zeros(m, m)) return row1.col_join(row2).col_join(row3) else: return row1.col_join(row2) @property def forcing(self): """Returns the forcing vector from 'lagranges_equations' method.""" if self.eom is None: raise ValueError('Need to compute the equations of motion first') return self._f_d @property def forcing_full(self): """Augments qdots to the forcing vector above.""" if self.eom is None: raise ValueError('Need to compute the equations of motion first') if self.coneqs: return self._qdots.col_join(self.forcing).col_join(self._f_cd) else: return self._qdots.col_join(self.forcing) 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 linearize(self, q_ind=None, qd_ind=None, q_dep=None, qd_dep=None, **kwargs): """Linearize the equations of motion about a symbolic operating point. If kwarg A_and_B is False (default), returns M, A, B, r for the linearized form, M*[q', u']^T = A*[q_ind, u_ind]^T + B*r. If kwarg A_and_B is True, returns A, B, r for the linearized form dx = A*x + B*r, where x = [q_ind, u_ind]^T. Note that this is computationally intensive if there are many symbolic parameters. For this reason, it may be more desirable to use the default A_and_B=False, returning M, A, and B. Values may then be substituted in to these matrices, and the state space form found as A = P.T*M.inv()*A, B = P.T*M.inv()*B, where P = Linearizer.perm_mat. In both cases, r is found as all dynamicsymbols in the equations of motion that are not part of q, u, q', or u'. They are sorted in canonical form. The operating points may be also entered using the ``op_point`` kwarg. This takes a dictionary of {symbol: value}, or a an iterable of such dictionaries. The values may be numberic or symbolic. The more values you can specify beforehand, the faster this computation will run. For more documentation, please see the ``Linearizer`` class.""" linearizer = self.to_linearizer(q_ind, qd_ind, q_dep, qd_dep) result = linearizer.linearize(**kwargs) return result + (linearizer.r,) def solve_multipliers(self, op_point=None, sol_type='dict'): """Solves for the values of the lagrange multipliers symbolically at the specified operating point Parameters ========== op_point : dict or iterable of dicts, optional Point at which to solve at. The operating point is specified as a dictionary or iterable of dictionaries of {symbol: value}. The value may be numeric or symbolic itself. sol_type : str, optional Solution return type. Valid options are: - 'dict': A dict of {symbol : value} (default) - 'Matrix': An ordered column matrix of the solution """ # Determine number of multipliers k = len(self.lam_vec) if k == 0: raise ValueError("System has no lagrange multipliers to solve for.") # Compose dict of operating conditions if isinstance(op_point, dict): op_point_dict = op_point elif iterable(op_point): op_point_dict = {} for op in op_point: op_point_dict.update(op) elif op_point is None: op_point_dict = {} else: raise TypeError("op_point must be either a dictionary or an " "iterable of dictionaries.") # Compose the system to be solved mass_matrix = self.mass_matrix.col_join((-self.lam_coeffs.row_join( zeros(k, k)))) force_matrix = self.forcing.col_join(self._f_cd) # Sub in the operating point mass_matrix = msubs(mass_matrix, op_point_dict) force_matrix = msubs(force_matrix, op_point_dict) # Solve for the multipliers sol_list = mass_matrix.LUsolve(-force_matrix)[-k:] if sol_type == 'dict': return dict(zip(self.lam_vec, sol_list)) elif sol_type == 'Matrix': return Matrix(sol_list) else: raise ValueError("Unknown sol_type {:}.".format(sol_type)) def rhs(self, inv_method=None, **kwargs): """Returns equations that can be solved numerically Parameters ========== inv_method : str The specific sympy inverse matrix calculation method to use. For a list of valid methods, see :meth:`~sympy.matrices.matrices.MatrixBase.inv` """ if 'method' in kwargs: # The method kwarg is deprecated in favor of inv_method. SymPyDeprecationWarning(feature="method kwarg", useinstead="inv_method kwarg", deprecated_since_version="0.7.6").warn() # For now accept both inv_method = kwargs['method'] if inv_method is None: self._rhs = self.mass_matrix_full.LUsolve(self.forcing_full) else: self._rhs = (self.mass_matrix_full.inv(inv_method, try_block_diag=True) * self.forcing_full) return self._rhs @property def q(self): return self._q @property def u(self): return self._qdots @property def bodies(self): return self._bodies @property def forcelist(self): return self._forcelist
def _form_block_matrices(self): """Form the block matrices for composing M, A, and B.""" # Extract dimension variables l, m, n, o, s, k = self._dims # Block Matrix Definitions. These are only defined if under certain # conditions. If undefined, an empty matrix is used instead if n != 0: self._M_qq = self.f_0.jacobian(self._qd) self._A_qq = -(self.f_0 + self.f_1).jacobian(self.q) else: self._M_qq = Matrix() self._A_qq = Matrix() if n != 0 and m != 0: self._M_uqc = self.f_a.jacobian(self._qd_dup) self._A_uqc = -self.f_a.jacobian(self.q) else: self._M_uqc = Matrix() self._A_uqc = Matrix() if n != 0 and o - m + k != 0: self._M_uqd = self.f_3.jacobian(self._qd_dup) self._A_uqd = -(self.f_2 + self.f_3 + self.f_4).jacobian(self.q) else: self._M_uqd = Matrix() self._A_uqd = Matrix() if o != 0 and m != 0: self._M_uuc = self.f_a.jacobian(self._ud) self._A_uuc = -self.f_a.jacobian(self.u) else: self._M_uuc = Matrix() self._A_uuc = Matrix() if o != 0 and o - m + k != 0: self._M_uud = self.f_2.jacobian(self._ud) self._A_uud = -(self.f_2 + self.f_3).jacobian(self.u) else: self._M_uud = Matrix() self._A_uud = Matrix() if o != 0 and n != 0: self._A_qu = -self.f_1.jacobian(self.u) else: self._A_qu = Matrix() if k != 0 and o - m + k != 0: self._M_uld = self.f_4.jacobian(self.lams) else: self._M_uld = Matrix() if s != 0 and o - m + k != 0: self._B_u = -self.f_3.jacobian(self.r) else: self._B_u = Matrix()
def __init__(self, f_0, f_1, f_2, f_3, f_4, f_c, f_v, f_a, q, u, q_i=None, q_d=None, u_i=None, u_d=None, r=None, lams=None): """ Parameters ---------- f_0, f_1, f_2, f_3, f_4, f_c, f_v, f_a : array_like System of equations holding the general system form. Supply empty array or Matrix if the parameter doesn't exist. q : array_like The generalized coordinates. u : array_like The generalized speeds q_i, u_i : array_like, optional The independent generalized coordinates and speeds. q_d, u_d : array_like, optional The dependent generalized coordinates and speeds. r : array_like, optional The input variables. lams : array_like, optional The lagrange multipliers """ # Generalized equation form self.f_0 = Matrix(f_0) self.f_1 = Matrix(f_1) self.f_2 = Matrix(f_2) self.f_3 = Matrix(f_3) self.f_4 = Matrix(f_4) self.f_c = Matrix(f_c) self.f_v = Matrix(f_v) self.f_a = Matrix(f_a) # Generalized equation variables self.q = Matrix(q) self.u = Matrix(u) none_handler = lambda x: Matrix(x) if x else Matrix() self.q_i = none_handler(q_i) self.q_d = none_handler(q_d) self.u_i = none_handler(u_i) self.u_d = none_handler(u_d) self.r = none_handler(r) self.lams = none_handler(lams) # Derivatives of generalized equation variables self._qd = self.q.diff(dynamicsymbols._t) self._ud = self.u.diff(dynamicsymbols._t) # If the user doesn't actually use generalized variables, and the # qd and u vectors have any intersecting variables, this can cause # problems. We'll fix this with some hackery, and Dummy variables dup_vars = set(self._qd).intersection(self.u) self._qd_dup = Matrix( [var if var not in dup_vars else Dummy() for var in self._qd]) # Derive dimesion terms l = len(self.f_c) m = len(self.f_v) n = len(self.q) o = len(self.u) s = len(self.r) k = len(self.lams) dims = namedtuple('dims', ['l', 'm', 'n', 'o', 's', 'k']) self._dims = dims(l, m, n, o, s, k) self._setup_done = False
def __init__(self, coord_states, right_hand_side, speeds=None, mass_matrix=None, coordinate_derivatives=None, alg_con=None, output_eqns={}, coord_idxs=None, speed_idxs=None, bodies=None, loads=None): """Initializes a SymbolicSystem object""" # Extract information on speeds, coordinates and states if speeds is None: self._states = Matrix(coord_states) if coord_idxs is None: self._coordinates = None else: coords = [coord_states[i] for i in coord_idxs] self._coordinates = Matrix(coords) if speed_idxs is None: self._speeds = None else: speeds_inter = [coord_states[i] for i in speed_idxs] self._speeds = Matrix(speeds_inter) else: self._coordinates = Matrix(coord_states) self._speeds = Matrix(speeds) self._states = self._coordinates.col_join(self._speeds) # Extract equations of motion form if coordinate_derivatives is not None: self._kin_explicit_rhs = coordinate_derivatives self._dyn_implicit_rhs = right_hand_side self._dyn_implicit_mat = mass_matrix self._comb_implicit_rhs = None self._comb_implicit_mat = None self._comb_explicit_rhs = None elif mass_matrix is not None: self._kin_explicit_rhs = None self._dyn_implicit_rhs = None self._dyn_implicit_mat = None self._comb_implicit_rhs = right_hand_side self._comb_implicit_mat = mass_matrix self._comb_explicit_rhs = None else: self._kin_explicit_rhs = None self._dyn_implicit_rhs = None self._dyn_implicit_mat = None self._comb_implicit_rhs = None self._comb_implicit_mat = None self._comb_explicit_rhs = right_hand_side # Set the remainder of the inputs as instance attributes if alg_con is not None and coordinate_derivatives is not None: alg_con = [i + len(coordinate_derivatives) for i in alg_con] self._alg_con = alg_con self.output_eqns = output_eqns # Change the body and loads iterables to tuples if they are not tuples # already if type(bodies) != tuple and bodies is not None: bodies = tuple(bodies) if type(loads) != tuple and loads is not None: loads = tuple(loads) self._bodies = bodies self._loads = loads
def test_linearize_pendulum_kane_nonminimal(): # Create generalized coordinates and speeds for this non-minimal realization # q1, q2 = N.x and N.y coordinates of pendulum # u1, u2 = N.x and N.y velocities of pendulum q1, q2 = dynamicsymbols('q1:3') q1d, q2d = dynamicsymbols('q1:3', level=1) u1, u2 = dynamicsymbols('u1:3') u1d, u2d = dynamicsymbols('u1:3', level=1) L, m, t = symbols('L, m, t') g = 9.8 # Compose world frame N = ReferenceFrame('N') pN = Point('N*') pN.set_vel(N, 0) # A.x is along the pendulum theta1 = atan(q2/q1) A = N.orientnew('A', 'axis', [theta1, N.z]) # Locate the pendulum mass P = pN.locatenew('P1', q1*N.x + q2*N.y) pP = Particle('pP', P, m) # Calculate the kinematic differential equations kde = Matrix([q1d - u1, q2d - u2]) dq_dict = solve(kde, [q1d, q2d]) # Set velocity of point P P.set_vel(N, P.pos_from(pN).dt(N).subs(dq_dict)) # Configuration constraint is length of pendulum f_c = Matrix([P.pos_from(pN).magnitude() - L]) # Velocity constraint is that the velocity in the A.x direction is # always zero (the pendulum is never getting longer). f_v = Matrix([P.vel(N).express(A).dot(A.x)]) f_v.simplify() # Acceleration constraints is the time derivative of the velocity constraint f_a = f_v.diff(t) f_a.simplify() # Input the force resultant at P R = m*g*N.x # Derive the equations of motion using the KanesMethod class. KM = KanesMethod(N, q_ind=[q2], u_ind=[u2], q_dependent=[q1], u_dependent=[u1], configuration_constraints=f_c, velocity_constraints=f_v, acceleration_constraints=f_a, kd_eqs=kde) with warnings.catch_warnings(): warnings.filterwarnings("ignore", category=SymPyDeprecationWarning) (fr, frstar) = KM.kanes_equations([(P, R)], [pP]) # Set the operating point to be straight down, and non-moving q_op = {q1: L, q2: 0} u_op = {u1: 0, u2: 0} ud_op = {u1d: 0, u2d: 0} A, B, inp_vec = KM.linearize(op_point=[q_op, u_op, ud_op], A_and_B=True, new_method=True, simplify=True) assert A.expand() == Matrix([[0, 1], [-9.8/L, 0]]) assert B == Matrix([])
def test_non_central_inertia(): # This tests that the calculation of Fr* does not depend the point # about which the inertia of a rigid body is defined. This test solves # exercises 8.12, 8.17 from Kane 1985. # Declare symbols q1, q2, q3 = dynamicsymbols('q1:4') q1d, q2d, q3d = dynamicsymbols('q1:4', level=1) u1, u2, u3, u4, u5 = dynamicsymbols('u1:6') u_prime, R, M, g, e, f, theta = symbols('u\' R, M, g, e, f, theta') a, b, mA, mB, IA, J, K, t = symbols('a b mA mB IA J K t') Q1, Q2, Q3 = symbols('Q1, Q2 Q3') IA22, IA23, IA33 = symbols('IA22 IA23 IA33') # Reference Frames F = ReferenceFrame('F') P = F.orientnew('P', 'axis', [-theta, F.y]) A = P.orientnew('A', 'axis', [q1, P.x]) A.set_ang_vel(F, u1 * A.x + u3 * A.z) # define frames for wheels B = A.orientnew('B', 'axis', [q2, A.z]) C = A.orientnew('C', 'axis', [q3, A.z]) B.set_ang_vel(A, u4 * A.z) C.set_ang_vel(A, u5 * A.z) # define points D, S*, Q on frame A and their velocities pD = Point('D') pD.set_vel(A, 0) # u3 will not change v_D_F since wheels are still assumed to roll without slip. pD.set_vel(F, u2 * A.y) pS_star = pD.locatenew('S*', e * A.y) pQ = pD.locatenew('Q', f * A.y - R * A.x) for p in [pS_star, pQ]: p.v2pt_theory(pD, F, A) # masscenters of bodies A, B, C pA_star = pD.locatenew('A*', a * A.y) pB_star = pD.locatenew('B*', b * A.z) pC_star = pD.locatenew('C*', -b * A.z) for p in [pA_star, pB_star, pC_star]: p.v2pt_theory(pD, F, A) # points of B, C touching the plane P pB_hat = pB_star.locatenew('B^', -R * A.x) pC_hat = pC_star.locatenew('C^', -R * A.x) pB_hat.v2pt_theory(pB_star, F, B) pC_hat.v2pt_theory(pC_star, F, C) # the velocities of B^, C^ are zero since B, C are assumed to roll without slip kde = [q1d - u1, q2d - u4, q3d - u5] vc = [dot(p.vel(F), A.y) for p in [pB_hat, pC_hat]] # inertias of bodies A, B, C # IA22, IA23, IA33 are not specified in the problem statement, but are # necessary to define an inertia object. Although the values of # IA22, IA23, IA33 are not known in terms of the variables given in the # problem statement, they do not appear in the general inertia terms. inertia_A = inertia(A, IA, IA22, IA33, 0, IA23, 0) inertia_B = inertia(B, K, K, J) inertia_C = inertia(C, K, K, J) # define the rigid bodies A, B, C rbA = RigidBody('rbA', pA_star, A, mA, (inertia_A, pA_star)) rbB = RigidBody('rbB', pB_star, B, mB, (inertia_B, pB_star)) rbC = RigidBody('rbC', pC_star, C, mB, (inertia_C, pC_star)) km = KanesMethod(F, q_ind=[q1, q2, q3], u_ind=[u1, u2], kd_eqs=kde, u_dependent=[u4, u5], velocity_constraints=vc, u_auxiliary=[u3]) forces = [(pS_star, -M * g * F.x), (pQ, Q1 * A.x + Q2 * A.y + Q3 * A.z)] bodies = [rbA, rbB, rbC] with warns_deprecated_sympy(): fr, fr_star = km.kanes_equations(forces, bodies) vc_map = solve(vc, [u4, u5]) # KanesMethod returns the negative of Fr, Fr* as defined in Kane1985. fr_star_expected = Matrix([ -(IA + 2 * J * b**2 / R**2 + 2 * K + mA * a**2 + 2 * mB * b**2) * u1.diff(t) - mA * a * u1 * u2, -(mA + 2 * mB + 2 * J / R**2) * u2.diff(t) + mA * a * u1**2, 0 ]) t = trigsimp(fr_star.subs(vc_map).subs({u3: 0})).doit().expand() assert ((fr_star_expected - t).expand() == zeros(3, 1)) # define inertias of rigid bodies A, B, C about point D # I_S/O = I_S/S* + I_S*/O bodies2 = [] for rb, I_star in zip([rbA, rbB, rbC], [inertia_A, inertia_B, inertia_C]): I = I_star + inertia_of_point_mass(rb.mass, rb.masscenter.pos_from(pD), rb.frame) bodies2.append(RigidBody('', rb.masscenter, rb.frame, rb.mass, (I, pD))) with warns_deprecated_sympy(): fr2, fr_star2 = km.kanes_equations(forces, bodies2) t = trigsimp(fr_star2.subs(vc_map).subs({u3: 0})).doit() assert (fr_star_expected - t).expand() == zeros(3, 1)
def mass_matrix(self): """The mass matrix of the system.""" if not self._fr or not self._frstar: raise ValueError('Need to compute Fr, Fr* first.') return Matrix([self._k_d, self._k_dnh])
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()
def test_aux_dep(): # This test is about rolling disc dynamics, comparing the results found # with KanesMethod to those found when deriving the equations "manually" # with SymPy. # The terms Fr, Fr*, and Fr*_steady are all compared between the two # methods. Here, Fr*_steady refers to the generalized inertia forces for an # equilibrium configuration. # Note: comparing to the test of test_rolling_disc() in test_kane.py, this # test also tests auxiliary speeds and configuration and motion constraints #, seen in the generalized dependent coordinates q[3], and depend speeds # u[3], u[4] and u[5]. # First, manual derivation of Fr, Fr_star, Fr_star_steady. # Symbols for time and constant parameters. # Symbols for contact forces: Fx, Fy, Fz. t, r, m, g, I, J = symbols('t r m g I J') Fx, Fy, Fz = symbols('Fx Fy Fz') # Configuration variables and their time derivatives: # q[0] -- yaw # q[1] -- lean # q[2] -- spin # q[3] -- dot(-r*B.z, A.z) -- distance from ground plane to disc center in # A.z direction # Generalized speeds and their time derivatives: # u[0] -- disc angular velocity component, disc fixed x direction # u[1] -- disc angular velocity component, disc fixed y direction # u[2] -- disc angular velocity component, disc fixed z direction # u[3] -- disc velocity component, A.x direction # u[4] -- disc velocity component, A.y direction # u[5] -- disc velocity component, A.z direction # Auxiliary generalized speeds: # ua[0] -- contact point auxiliary generalized speed, A.x direction # ua[1] -- contact point auxiliary generalized speed, A.y direction # ua[2] -- contact point auxiliary generalized speed, A.z direction q = dynamicsymbols('q:4') qd = [qi.diff(t) for qi in q] u = dynamicsymbols('u:6') ud = [ui.diff(t) for ui in u] ud_zero = dict(zip(ud, [0.] * len(ud))) ua = dynamicsymbols('ua:3') ua_zero = dict(zip(ua, [0.] * len(ua))) # noqa:F841 # Reference frames: # Yaw intermediate frame: A. # Lean intermediate frame: B. # Disc fixed frame: C. N = ReferenceFrame('N') A = N.orientnew('A', 'Axis', [q[0], N.z]) B = A.orientnew('B', 'Axis', [q[1], A.x]) C = B.orientnew('C', 'Axis', [q[2], B.y]) # Angular velocity and angular acceleration of disc fixed frame # u[0], u[1] and u[2] are generalized independent speeds. C.set_ang_vel(N, u[0] * B.x + u[1] * B.y + u[2] * B.z) C.set_ang_acc( N, C.ang_vel_in(N).diff(t, B) + cross(B.ang_vel_in(N), C.ang_vel_in(N))) # Velocity and acceleration of points: # Disc-ground contact point: P. # Center of disc: O, defined from point P with depend coordinate: q[3] # u[3], u[4] and u[5] are generalized dependent speeds. P = Point('P') P.set_vel(N, ua[0] * A.x + ua[1] * A.y + ua[2] * A.z) O = P.locatenew('O', q[3] * A.z + r * sin(q[1]) * A.y) O.set_vel(N, u[3] * A.x + u[4] * A.y + u[5] * A.z) O.set_acc(N, O.vel(N).diff(t, A) + cross(A.ang_vel_in(N), O.vel(N))) # Kinematic differential equations: # Two equalities: one is w_c_n_qd = C.ang_vel_in(N) in three coordinates # directions of B, for qd0, qd1 and qd2. # the other is v_o_n_qd = O.vel(N) in A.z direction for qd3. # Then, solve for dq/dt's in terms of u's: qd_kd. w_c_n_qd = qd[0] * A.z + qd[1] * B.x + qd[2] * B.y v_o_n_qd = O.pos_from(P).diff(t, A) + cross(A.ang_vel_in(N), O.pos_from(P)) kindiffs = Matrix([dot(w_c_n_qd - C.ang_vel_in(N), uv) for uv in B] + [dot(v_o_n_qd - O.vel(N), A.z)]) qd_kd = solve(kindiffs, qd) # noqa:F841 # Values of generalized speeds during a steady turn for later substitution # into the Fr_star_steady. steady_conditions = solve(kindiffs.subs({qd[1]: 0, qd[3]: 0}), u) steady_conditions.update({qd[1]: 0, qd[3]: 0}) # Partial angular velocities and velocities. partial_w_C = [C.ang_vel_in(N).diff(ui, N) for ui in u + ua] partial_v_O = [O.vel(N).diff(ui, N) for ui in u + ua] partial_v_P = [P.vel(N).diff(ui, N) for ui in u + ua] # Configuration constraint: f_c, the projection of radius r in A.z direction # is q[3]. # Velocity constraints: f_v, for u3, u4 and u5. # Acceleration constraints: f_a. f_c = Matrix([dot(-r * B.z, A.z) - q[3]]) f_v = Matrix([ dot(O.vel(N) - (P.vel(N) + cross(C.ang_vel_in(N), O.pos_from(P))), ai).expand() for ai in A ]) v_o_n = cross(C.ang_vel_in(N), O.pos_from(P)) a_o_n = v_o_n.diff(t, A) + cross(A.ang_vel_in(N), v_o_n) f_a = Matrix([dot(O.acc(N) - a_o_n, ai) for ai in A]) # noqa:F841 # Solve for constraint equations in the form of # u_dependent = A_rs * [u_i; u_aux]. # First, obtain constraint coefficient matrix: M_v * [u; ua] = 0; # Second, taking u[0], u[1], u[2] as independent, # taking u[3], u[4], u[5] as dependent, # rearranging the matrix of M_v to be A_rs for u_dependent. # Third, u_aux ==0 for u_dep, and resulting dictionary of u_dep_dict. M_v = zeros(3, 9) for i in range(3): for j, ui in enumerate(u + ua): M_v[i, j] = f_v[i].diff(ui) M_v_i = M_v[:, :3] M_v_d = M_v[:, 3:6] M_v_aux = M_v[:, 6:] M_v_i_aux = M_v_i.row_join(M_v_aux) A_rs = -M_v_d.inv() * M_v_i_aux u_dep = A_rs[:, :3] * Matrix(u[:3]) u_dep_dict = dict(zip(u[3:], u_dep)) # Active forces: F_O acting on point O; F_P acting on point P. # Generalized active forces (unconstrained): Fr_u = F_point * pv_point. F_O = m * g * A.z F_P = Fx * A.x + Fy * A.y + Fz * A.z Fr_u = Matrix([ dot(F_O, pv_o) + dot(F_P, pv_p) for pv_o, pv_p in zip(partial_v_O, partial_v_P) ]) # Inertia force: R_star_O. # Inertia of disc: I_C_O, where J is a inertia component about principal axis. # Inertia torque: T_star_C. # Generalized inertia forces (unconstrained): Fr_star_u. R_star_O = -m * O.acc(N) I_C_O = inertia(B, I, J, I) T_star_C = -(dot(I_C_O, C.ang_acc_in(N)) \ + cross(C.ang_vel_in(N), dot(I_C_O, C.ang_vel_in(N)))) Fr_star_u = Matrix([ dot(R_star_O, pv) + dot(T_star_C, pav) for pv, pav in zip(partial_v_O, partial_w_C) ]) # Form nonholonomic Fr: Fr_c, and nonholonomic Fr_star: Fr_star_c. # Also, nonholonomic Fr_star in steady turning condition: Fr_star_steady. Fr_c = Fr_u[:3, :].col_join(Fr_u[6:, :]) + A_rs.T * Fr_u[3:6, :] Fr_star_c = Fr_star_u[:3, :].col_join(Fr_star_u[6:, :])\ + A_rs.T * Fr_star_u[3:6, :] Fr_star_steady = Fr_star_c.subs(ud_zero).subs(u_dep_dict)\ .subs(steady_conditions).subs({q[3]: -r*cos(q[1])}).expand() # Second, using KaneMethod in mechanics for fr, frstar and frstar_steady. # Rigid Bodies: disc, with inertia I_C_O. iner_tuple = (I_C_O, O) disc = RigidBody('disc', O, C, m, iner_tuple) bodyList = [disc] # Generalized forces: Gravity: F_o; Auxiliary forces: F_p. F_o = (O, F_O) F_p = (P, F_P) forceList = [F_o, F_p] # KanesMethod. kane = KanesMethod(N, q_ind=q[:3], u_ind=u[:3], kd_eqs=kindiffs, q_dependent=q[3:], configuration_constraints=f_c, u_dependent=u[3:], velocity_constraints=f_v, u_auxiliary=ua) # fr, frstar, frstar_steady and kdd(kinematic differential equations). with warns_deprecated_sympy(): (fr, frstar) = kane.kanes_equations(forceList, bodyList) frstar_steady = frstar.subs(ud_zero).subs(u_dep_dict).subs(steady_conditions)\ .subs({q[3]: -r*cos(q[1])}).expand() kdd = kane.kindiffdict() assert Matrix(Fr_c).expand() == fr.expand() assert Matrix(Fr_star_c.subs(kdd)).expand() == frstar.expand() assert (simplify(Matrix(Fr_star_steady).expand()) == simplify( frstar_steady.expand())) syms_in_forcing = find_dynamicsymbols(kane.forcing) for qdi in qd: assert qdi not in syms_in_forcing
def test_sub_qdot2(): # This test solves exercises 8.3 from Kane 1985 and defines # all velocities in terms of q, qdot. We check that the generalized active # forces are correctly computed if u terms are only defined in the # kinematic differential equations. # # This functionality was added in PR 8948. Without qdot/u substitution, the # KanesMethod constructor will fail during the constraint initialization as # the B matrix will be poorly formed and inversion of the dependent part # will fail. g, m, Px, Py, Pz, R, t = symbols('g m Px Py Pz R t') q = dynamicsymbols('q:5') qd = dynamicsymbols('q:5', level=1) u = dynamicsymbols('u:5') ## Define inertial, intermediate, and rigid body reference frames A = ReferenceFrame('A') B_prime = A.orientnew('B_prime', 'Axis', [q[0], A.z]) B = B_prime.orientnew('B', 'Axis', [pi/2 - q[1], B_prime.x]) C = B.orientnew('C', 'Axis', [q[2], B.z]) ## Define points of interest and their velocities pO = Point('O') pO.set_vel(A, 0) # R is the point in plane H that comes into contact with disk C. pR = pO.locatenew('R', q[3]*A.x + q[4]*A.y) pR.set_vel(A, pR.pos_from(pO).diff(t, A)) pR.set_vel(B, 0) # C^ is the point in disk C that comes into contact with plane H. pC_hat = pR.locatenew('C^', 0) pC_hat.set_vel(C, 0) # C* is the point at the center of disk C. pCs = pC_hat.locatenew('C*', R*B.y) pCs.set_vel(C, 0) pCs.set_vel(B, 0) # calculate velocites of points C* and C^ in frame A pCs.v2pt_theory(pR, A, B) # points C* and R are fixed in frame B pC_hat.v2pt_theory(pCs, A, C) # points C* and C^ are fixed in frame C ## Define forces on each point of the system R_C_hat = Px*A.x + Py*A.y + Pz*A.z R_Cs = -m*g*A.z forces = [(pC_hat, R_C_hat), (pCs, R_Cs)] ## Define kinematic differential equations # let ui = omega_C_A & bi (i = 1, 2, 3) # u4 = qd4, u5 = qd5 u_expr = [C.ang_vel_in(A) & uv for uv in B] u_expr += qd[3:] kde = [ui - e for ui, e in zip(u, u_expr)] km1 = KanesMethod(A, q, u, kde) with warnings.catch_warnings(): warnings.filterwarnings("ignore", category=SymPyDeprecationWarning) fr1, _ = km1.kanes_equations(forces, []) ## Calculate generalized active forces if we impose the condition that the # disk C is rolling without slipping u_indep = u[:3] u_dep = list(set(u) - set(u_indep)) vc = [pC_hat.vel(A) & uv for uv in [A.x, A.y]] km2 = KanesMethod(A, q, u_indep, kde, u_dependent=u_dep, velocity_constraints=vc) with warnings.catch_warnings(): warnings.filterwarnings("ignore", category=SymPyDeprecationWarning) fr2, _ = km2.kanes_equations(forces, []) fr1_expected = Matrix([ -R*g*m*sin(q[1]), -R*(Px*cos(q[0]) + Py*sin(q[0]))*tan(q[1]), R*(Px*cos(q[0]) + Py*sin(q[0])), Px, Py]) fr2_expected = Matrix([ -R*g*m*sin(q[1]), 0, 0]) assert (trigsimp(fr1.expand()) == trigsimp(fr1_expected.expand())) assert (trigsimp(fr2.expand()) == trigsimp(fr2_expected.expand()))
def test_sub_qdot(): # This test solves exercises 8.12, 8.17 from Kane 1985 and defines # some velocities in terms of q, qdot. ## --- Declare symbols --- q1, q2, q3 = dynamicsymbols('q1:4') q1d, q2d, q3d = dynamicsymbols('q1:4', level=1) u1, u2, u3 = dynamicsymbols('u1:4') u_prime, R, M, g, e, f, theta = symbols('u\' R, M, g, e, f, theta') a, b, mA, mB, IA, J, K, t = symbols('a b mA mB IA J K t') IA22, IA23, IA33 = symbols('IA22 IA23 IA33') Q1, Q2, Q3 = symbols('Q1 Q2 Q3') # --- Reference Frames --- F = ReferenceFrame('F') P = F.orientnew('P', 'axis', [-theta, F.y]) A = P.orientnew('A', 'axis', [q1, P.x]) A.set_ang_vel(F, u1*A.x + u3*A.z) # define frames for wheels B = A.orientnew('B', 'axis', [q2, A.z]) C = A.orientnew('C', 'axis', [q3, A.z]) ## --- define points D, S*, Q on frame A and their velocities --- pD = Point('D') pD.set_vel(A, 0) # u3 will not change v_D_F since wheels are still assumed to roll w/o slip pD.set_vel(F, u2 * A.y) pS_star = pD.locatenew('S*', e*A.y) pQ = pD.locatenew('Q', f*A.y - R*A.x) # masscenters of bodies A, B, C pA_star = pD.locatenew('A*', a*A.y) pB_star = pD.locatenew('B*', b*A.z) pC_star = pD.locatenew('C*', -b*A.z) for p in [pS_star, pQ, pA_star, pB_star, pC_star]: p.v2pt_theory(pD, F, A) # points of B, C touching the plane P pB_hat = pB_star.locatenew('B^', -R*A.x) pC_hat = pC_star.locatenew('C^', -R*A.x) pB_hat.v2pt_theory(pB_star, F, B) pC_hat.v2pt_theory(pC_star, F, C) # --- relate qdot, u --- # the velocities of B^, C^ are zero since B, C are assumed to roll w/o slip kde = [dot(p.vel(F), A.y) for p in [pB_hat, pC_hat]] kde += [u1 - q1d] kde_map = solve(kde, [q1d, q2d, q3d]) for k, v in list(kde_map.items()): kde_map[k.diff(t)] = v.diff(t) # inertias of bodies A, B, C # IA22, IA23, IA33 are not specified in the problem statement, but are # necessary to define an inertia object. Although the values of # IA22, IA23, IA33 are not known in terms of the variables given in the # problem statement, they do not appear in the general inertia terms. inertia_A = inertia(A, IA, IA22, IA33, 0, IA23, 0) inertia_B = inertia(B, K, K, J) inertia_C = inertia(C, K, K, J) # define the rigid bodies A, B, C rbA = RigidBody('rbA', pA_star, A, mA, (inertia_A, pA_star)) rbB = RigidBody('rbB', pB_star, B, mB, (inertia_B, pB_star)) rbC = RigidBody('rbC', pC_star, C, mB, (inertia_C, pC_star)) ## --- use kanes method --- km = KanesMethod(F, [q1, q2, q3], [u1, u2], kd_eqs=kde, u_auxiliary=[u3]) forces = [(pS_star, -M*g*F.x), (pQ, Q1*A.x + Q2*A.y + Q3*A.z)] bodies = [rbA, rbB, rbC] # Q2 = -u_prime * u2 * Q1 / sqrt(u2**2 + f**2 * u1**2) # -u_prime * R * u2 / sqrt(u2**2 + f**2 * u1**2) = R / Q1 * Q2 fr_expected = Matrix([ f*Q3 + M*g*e*sin(theta)*cos(q1), Q2 + M*g*sin(theta)*sin(q1), e*M*g*cos(theta) - Q1*f - Q2*R]) #Q1 * (f - u_prime * R * u2 / sqrt(u2**2 + f**2 * u1**2)))]) fr_star_expected = Matrix([ -(IA + 2*J*b**2/R**2 + 2*K + mA*a**2 + 2*mB*b**2) * u1.diff(t) - mA*a*u1*u2, -(mA + 2*mB +2*J/R**2) * u2.diff(t) + mA*a*u1**2, 0]) with warnings.catch_warnings(): warnings.filterwarnings("ignore", category=SymPyDeprecationWarning) fr, fr_star = km.kanes_equations(forces, bodies) assert (fr.expand() == fr_expected.expand()) assert ((fr_star_expected - trigsimp(fr_star)).expand() == zeros(3, 1))
def test_aux_dep(): # This test is about rolling disc dynamics, comparing the results found # with KanesMethod to those found when deriving the equations "manually" # with SymPy. # The terms Fr, Fr*, and Fr*_steady are all compared between the two # methods. Here, Fr*_steady refers to the generalized inertia forces for an # equilibrium configuration. # Note: comparing to the test of test_rolling_disc() in test_kane.py, this # test also tests auxiliary speeds and configuration and motion constraints #, seen in the generalized dependent coordinates q[3], and depend speeds # u[3], u[4] and u[5]. # First, manual derivation of Fr, Fr_star, Fr_star_steady. # Symbols for time and constant parameters. # Symbols for contact forces: Fx, Fy, Fz. t, r, m, g, I, J = symbols('t r m g I J') Fx, Fy, Fz = symbols('Fx Fy Fz') # Configuration variables and their time derivatives: # q[0] -- yaw # q[1] -- lean # q[2] -- spin # q[3] -- dot(-r*B.z, A.z) -- distance from ground plane to disc center in # A.z direction # Generalized speeds and their time derivatives: # u[0] -- disc angular velocity component, disc fixed x direction # u[1] -- disc angular velocity component, disc fixed y direction # u[2] -- disc angular velocity component, disc fixed z direction # u[3] -- disc velocity component, A.x direction # u[4] -- disc velocity component, A.y direction # u[5] -- disc velocity component, A.z direction # Auxiliary generalized speeds: # ua[0] -- contact point auxiliary generalized speed, A.x direction # ua[1] -- contact point auxiliary generalized speed, A.y direction # ua[2] -- contact point auxiliary generalized speed, A.z direction q = dynamicsymbols('q:4') qd = [qi.diff(t) for qi in q] u = dynamicsymbols('u:6') ud = [ui.diff(t) for ui in u] ud_zero = dict(zip(ud, [0.]*len(ud))) ua = dynamicsymbols('ua:3') ua_zero = dict(zip(ua, [0.]*len(ua))) # Reference frames: # Yaw intermediate frame: A. # Lean intermediate frame: B. # Disc fixed frame: C. N = ReferenceFrame('N') A = N.orientnew('A', 'Axis', [q[0], N.z]) B = A.orientnew('B', 'Axis', [q[1], A.x]) C = B.orientnew('C', 'Axis', [q[2], B.y]) # Angular velocity and angular acceleration of disc fixed frame # u[0], u[1] and u[2] are generalized independent speeds. C.set_ang_vel(N, u[0]*B.x + u[1]*B.y + u[2]*B.z) C.set_ang_acc(N, C.ang_vel_in(N).diff(t, B) + cross(B.ang_vel_in(N), C.ang_vel_in(N))) # Velocity and acceleration of points: # Disc-ground contact point: P. # Center of disc: O, defined from point P with depend coordinate: q[3] # u[3], u[4] and u[5] are generalized dependent speeds. P = Point('P') P.set_vel(N, ua[0]*A.x + ua[1]*A.y + ua[2]*A.z) O = P.locatenew('O', q[3]*A.z + r*sin(q[1])*A.y) O.set_vel(N, u[3]*A.x + u[4]*A.y + u[5]*A.z) O.set_acc(N, O.vel(N).diff(t, A) + cross(A.ang_vel_in(N), O.vel(N))) # Kinematic differential equations: # Two equalities: one is w_c_n_qd = C.ang_vel_in(N) in three coordinates # directions of B, for qd0, qd1 and qd2. # the other is v_o_n_qd = O.vel(N) in A.z direction for qd3. # Then, solve for dq/dt's in terms of u's: qd_kd. w_c_n_qd = qd[0]*A.z + qd[1]*B.x + qd[2]*B.y v_o_n_qd = O.pos_from(P).diff(t, A) + cross(A.ang_vel_in(N), O.pos_from(P)) kindiffs = Matrix([dot(w_c_n_qd - C.ang_vel_in(N), uv) for uv in B] + [dot(v_o_n_qd - O.vel(N), A.z)]) qd_kd = solve(kindiffs, qd) # Values of generalized speeds during a steady turn for later substitution # into the Fr_star_steady. steady_conditions = solve(kindiffs.subs({qd[1] : 0, qd[3] : 0}), u) steady_conditions.update({qd[1] : 0, qd[3] : 0}) # Partial angular velocities and velocities. partial_w_C = [C.ang_vel_in(N).diff(ui, N) for ui in u + ua] partial_v_O = [O.vel(N).diff(ui, N) for ui in u + ua] partial_v_P = [P.vel(N).diff(ui, N) for ui in u + ua] # Configuration constraint: f_c, the projection of radius r in A.z direction # is q[3]. # Velocity constraints: f_v, for u3, u4 and u5. # Acceleration constraints: f_a. f_c = Matrix([dot(-r*B.z, A.z) - q[3]]) f_v = Matrix([dot(O.vel(N) - (P.vel(N) + cross(C.ang_vel_in(N), O.pos_from(P))), ai).expand() for ai in A]) v_o_n = cross(C.ang_vel_in(N), O.pos_from(P)) a_o_n = v_o_n.diff(t, A) + cross(A.ang_vel_in(N), v_o_n) f_a = Matrix([dot(O.acc(N) - a_o_n, ai) for ai in A]) # Solve for constraint equations in the form of # u_dependent = A_rs * [u_i; u_aux]. # First, obtain constraint coefficient matrix: M_v * [u; ua] = 0; # Second, taking u[0], u[1], u[2] as independent, # taking u[3], u[4], u[5] as dependent, # rearranging the matrix of M_v to be A_rs for u_dependent. # Third, u_aux ==0 for u_dep, and resulting dictionary of u_dep_dict. M_v = zeros(3, 9) for i in range(3): for j, ui in enumerate(u + ua): M_v[i, j] = f_v[i].diff(ui) M_v_i = M_v[:, :3] M_v_d = M_v[:, 3:6] M_v_aux = M_v[:, 6:] M_v_i_aux = M_v_i.row_join(M_v_aux) A_rs = - M_v_d.inv() * M_v_i_aux u_dep = A_rs[:, :3] * Matrix(u[:3]) u_dep_dict = dict(zip(u[3:], u_dep)) # Active forces: F_O acting on point O; F_P acting on point P. # Generalized active forces (unconstrained): Fr_u = F_point * pv_point. F_O = m*g*A.z F_P = Fx * A.x + Fy * A.y + Fz * A.z Fr_u = Matrix([dot(F_O, pv_o) + dot(F_P, pv_p) for pv_o, pv_p in zip(partial_v_O, partial_v_P)]) # Inertia force: R_star_O. # Inertia of disc: I_C_O, where J is a inertia component about principal axis. # Inertia torque: T_star_C. # Generalized inertia forces (unconstrained): Fr_star_u. R_star_O = -m*O.acc(N) I_C_O = inertia(B, I, J, I) T_star_C = -(dot(I_C_O, C.ang_acc_in(N)) \ + cross(C.ang_vel_in(N), dot(I_C_O, C.ang_vel_in(N)))) Fr_star_u = Matrix([dot(R_star_O, pv) + dot(T_star_C, pav) for pv, pav in zip(partial_v_O, partial_w_C)]) # Form nonholonomic Fr: Fr_c, and nonholonomic Fr_star: Fr_star_c. # Also, nonholonomic Fr_star in steady turning condition: Fr_star_steady. Fr_c = Fr_u[:3, :].col_join(Fr_u[6:, :]) + A_rs.T * Fr_u[3:6, :] Fr_star_c = Fr_star_u[:3, :].col_join(Fr_star_u[6:, :])\ + A_rs.T * Fr_star_u[3:6, :] Fr_star_steady = Fr_star_c.subs(ud_zero).subs(u_dep_dict)\ .subs(steady_conditions).subs({q[3]: -r*cos(q[1])}).expand() # Second, using KaneMethod in mechanics for fr, frstar and frstar_steady. # Rigid Bodies: disc, with inertia I_C_O. iner_tuple = (I_C_O, O) disc = RigidBody('disc', O, C, m, iner_tuple) bodyList = [disc] # Generalized forces: Gravity: F_o; Auxiliary forces: F_p. F_o = (O, F_O) F_p = (P, F_P) forceList = [F_o, F_p] # KanesMethod. kane = KanesMethod( N, q_ind= q[:3], u_ind= u[:3], kd_eqs=kindiffs, q_dependent=q[3:], configuration_constraints = f_c, u_dependent=u[3:], velocity_constraints= f_v, u_auxiliary=ua ) # fr, frstar, frstar_steady and kdd(kinematic differential equations). with warnings.catch_warnings(): warnings.filterwarnings("ignore", category=SymPyDeprecationWarning) (fr, frstar)= kane.kanes_equations(forceList, bodyList) frstar_steady = frstar.subs(ud_zero).subs(u_dep_dict).subs(steady_conditions)\ .subs({q[3]: -r*cos(q[1])}).expand() kdd = kane.kindiffdict() assert Matrix(Fr_c).expand() == fr.expand() assert Matrix(Fr_star_c.subs(kdd)).expand() == frstar.expand() assert (simplify(Matrix(Fr_star_steady).expand()) == simplify(frstar_steady.expand()))
def kinematic_equations(speeds, coords, rot_type, rot_order=''): """Gives equations relating the qdot's to u's for a rotation type. Supply rotation type and order as in orient. Speeds are assumed to be body-fixed; if we are defining the orientation of B in A using by rot_type, the angular velocity of B in A is assumed to be in the form: speed[0]*B.x + speed[1]*B.y + speed[2]*B.z Parameters ========== speeds : list of length 3 The body fixed angular velocity measure numbers. coords : list of length 3 or 4 The coordinates used to define the orientation of the two frames. rot_type : str The type of rotation used to create the equations. Body, Space, or Quaternion only rot_order : str or int If applicable, the order of a series of rotations. Examples ======== >>> from sympy.physics.vector import dynamicsymbols >>> from sympy.physics.vector import kinematic_equations, vprint >>> u1, u2, u3 = dynamicsymbols('u1 u2 u3') >>> q1, q2, q3 = dynamicsymbols('q1 q2 q3') >>> vprint(kinematic_equations([u1,u2,u3], [q1,q2,q3], 'body', '313'), ... order=None) [-(u1*sin(q3) + u2*cos(q3))/sin(q2) + q1', -u1*cos(q3) + u2*sin(q3) + q2', (u1*sin(q3) + u2*cos(q3))*cos(q2)/sin(q2) - u3 + q3'] """ # Code below is checking and sanitizing input approved_orders = ('123', '231', '312', '132', '213', '321', '121', '131', '212', '232', '313', '323', '1', '2', '3', '') # make sure XYZ => 123 and rot_type is in lower case rot_order = translate(str(rot_order), 'XYZxyz', '123123') rot_type = rot_type.lower() if not isinstance(speeds, (list, tuple)): raise TypeError('Need to supply speeds in a list') if len(speeds) != 3: raise TypeError('Need to supply 3 body-fixed speeds') if not isinstance(coords, (list, tuple)): raise TypeError('Need to supply coordinates in a list') if rot_type in ['body', 'space']: if rot_order not in approved_orders: raise ValueError('Not an acceptable rotation order') if len(coords) != 3: raise ValueError('Need 3 coordinates for body or space') # Actual hard-coded kinematic differential equations w1, w2, w3 = speeds if w1 == w2 == w3 == 0: return [S.Zero] * 3 q1, q2, q3 = coords q1d, q2d, q3d = [diff(i, dynamicsymbols._t) for i in coords] s1, s2, s3 = [sin(q1), sin(q2), sin(q3)] c1, c2, c3 = [cos(q1), cos(q2), cos(q3)] if rot_type == 'body': if rot_order == '123': return [ q1d - (w1 * c3 - w2 * s3) / c2, q2d - w1 * s3 - w2 * c3, q3d - (-w1 * c3 + w2 * s3) * s2 / c2 - w3 ] if rot_order == '231': return [ q1d - (w2 * c3 - w3 * s3) / c2, q2d - w2 * s3 - w3 * c3, q3d - w1 - (-w2 * c3 + w3 * s3) * s2 / c2 ] if rot_order == '312': return [ q1d - (-w1 * s3 + w3 * c3) / c2, q2d - w1 * c3 - w3 * s3, q3d - (w1 * s3 - w3 * c3) * s2 / c2 - w2 ] if rot_order == '132': return [ q1d - (w1 * c3 + w3 * s3) / c2, q2d + w1 * s3 - w3 * c3, q3d - (w1 * c3 + w3 * s3) * s2 / c2 - w2 ] if rot_order == '213': return [ q1d - (w1 * s3 + w2 * c3) / c2, q2d - w1 * c3 + w2 * s3, q3d - (w1 * s3 + w2 * c3) * s2 / c2 - w3 ] if rot_order == '321': return [ q1d - (w2 * s3 + w3 * c3) / c2, q2d - w2 * c3 + w3 * s3, q3d - w1 - (w2 * s3 + w3 * c3) * s2 / c2 ] if rot_order == '121': return [ q1d - (w2 * s3 + w3 * c3) / s2, q2d - w2 * c3 + w3 * s3, q3d - w1 + (w2 * s3 + w3 * c3) * c2 / s2 ] if rot_order == '131': return [ q1d - (-w2 * c3 + w3 * s3) / s2, q2d - w2 * s3 - w3 * c3, q3d - w1 - (w2 * c3 - w3 * s3) * c2 / s2 ] if rot_order == '212': return [ q1d - (w1 * s3 - w3 * c3) / s2, q2d - w1 * c3 - w3 * s3, q3d - (-w1 * s3 + w3 * c3) * c2 / s2 - w2 ] if rot_order == '232': return [ q1d - (w1 * c3 + w3 * s3) / s2, q2d + w1 * s3 - w3 * c3, q3d + (w1 * c3 + w3 * s3) * c2 / s2 - w2 ] if rot_order == '313': return [ q1d - (w1 * s3 + w2 * c3) / s2, q2d - w1 * c3 + w2 * s3, q3d + (w1 * s3 + w2 * c3) * c2 / s2 - w3 ] if rot_order == '323': return [ q1d - (-w1 * c3 + w2 * s3) / s2, q2d - w1 * s3 - w2 * c3, q3d - (w1 * c3 - w2 * s3) * c2 / s2 - w3 ] if rot_type == 'space': if rot_order == '123': return [ q1d - w1 - (w2 * s1 + w3 * c1) * s2 / c2, q2d - w2 * c1 + w3 * s1, q3d - (w2 * s1 + w3 * c1) / c2 ] if rot_order == '231': return [ q1d - (w1 * c1 + w3 * s1) * s2 / c2 - w2, q2d + w1 * s1 - w3 * c1, q3d - (w1 * c1 + w3 * s1) / c2 ] if rot_order == '312': return [ q1d - (w1 * s1 + w2 * c1) * s2 / c2 - w3, q2d - w1 * c1 + w2 * s1, q3d - (w1 * s1 + w2 * c1) / c2 ] if rot_order == '132': return [ q1d - w1 - (-w2 * c1 + w3 * s1) * s2 / c2, q2d - w2 * s1 - w3 * c1, q3d - (w2 * c1 - w3 * s1) / c2 ] if rot_order == '213': return [ q1d - (w1 * s1 - w3 * c1) * s2 / c2 - w2, q2d - w1 * c1 - w3 * s1, q3d - (-w1 * s1 + w3 * c1) / c2 ] if rot_order == '321': return [ q1d - (-w1 * c1 + w2 * s1) * s2 / c2 - w3, q2d - w1 * s1 - w2 * c1, q3d - (w1 * c1 - w2 * s1) / c2 ] if rot_order == '121': return [ q1d - w1 + (w2 * s1 + w3 * c1) * c2 / s2, q2d - w2 * c1 + w3 * s1, q3d - (w2 * s1 + w3 * c1) / s2 ] if rot_order == '131': return [ q1d - w1 - (w2 * c1 - w3 * s1) * c2 / s2, q2d - w2 * s1 - w3 * c1, q3d - (-w2 * c1 + w3 * s1) / s2 ] if rot_order == '212': return [ q1d - (-w1 * s1 + w3 * c1) * c2 / s2 - w2, q2d - w1 * c1 - w3 * s1, q3d - (w1 * s1 - w3 * c1) / s2 ] if rot_order == '232': return [ q1d + (w1 * c1 + w3 * s1) * c2 / s2 - w2, q2d + w1 * s1 - w3 * c1, q3d - (w1 * c1 + w3 * s1) / s2 ] if rot_order == '313': return [ q1d + (w1 * s1 + w2 * c1) * c2 / s2 - w3, q2d - w1 * c1 + w2 * s1, q3d - (w1 * s1 + w2 * c1) / s2 ] if rot_order == '323': return [ q1d - (w1 * c1 - w2 * s1) * c2 / s2 - w3, q2d - w1 * s1 - w2 * c1, q3d - (-w1 * c1 + w2 * s1) / s2 ] elif rot_type == 'quaternion': if rot_order != '': raise ValueError('Cannot have rotation order for quaternion') if len(coords) != 4: raise ValueError('Need 4 coordinates for quaternion') # Actual hard-coded kinematic differential equations e0, e1, e2, e3 = coords w = Matrix(speeds + [0]) E = Matrix([[e0, -e3, e2, e1], [e3, e0, -e1, e2], [-e2, e1, e0, e3], [-e1, -e2, -e3, e0]]) edots = Matrix([diff(i, dynamicsymbols._t) for i in [e1, e2, e3, e0]]) return list(edots.T - 0.5 * w.T * E.T) else: raise ValueError('Not an approved rotation type for this function')
def test_sub_qdot(): # This test solves exercises 8.12, 8.17 from Kane 1985 and defines # some velocities in terms of q, qdot. ## --- Declare symbols --- q1, q2, q3 = dynamicsymbols('q1:4') q1d, q2d, q3d = dynamicsymbols('q1:4', level=1) u1, u2, u3 = dynamicsymbols('u1:4') u_prime, R, M, g, e, f, theta = symbols('u\' R, M, g, e, f, theta') a, b, mA, mB, IA, J, K, t = symbols('a b mA mB IA J K t') IA22, IA23, IA33 = symbols('IA22 IA23 IA33') Q1, Q2, Q3 = symbols('Q1 Q2 Q3') # --- Reference Frames --- F = ReferenceFrame('F') P = F.orientnew('P', 'axis', [-theta, F.y]) A = P.orientnew('A', 'axis', [q1, P.x]) A.set_ang_vel(F, u1 * A.x + u3 * A.z) # define frames for wheels B = A.orientnew('B', 'axis', [q2, A.z]) C = A.orientnew('C', 'axis', [q3, A.z]) ## --- define points D, S*, Q on frame A and their velocities --- pD = Point('D') pD.set_vel(A, 0) # u3 will not change v_D_F since wheels are still assumed to roll w/o slip pD.set_vel(F, u2 * A.y) pS_star = pD.locatenew('S*', e * A.y) pQ = pD.locatenew('Q', f * A.y - R * A.x) # masscenters of bodies A, B, C pA_star = pD.locatenew('A*', a * A.y) pB_star = pD.locatenew('B*', b * A.z) pC_star = pD.locatenew('C*', -b * A.z) for p in [pS_star, pQ, pA_star, pB_star, pC_star]: p.v2pt_theory(pD, F, A) # points of B, C touching the plane P pB_hat = pB_star.locatenew('B^', -R * A.x) pC_hat = pC_star.locatenew('C^', -R * A.x) pB_hat.v2pt_theory(pB_star, F, B) pC_hat.v2pt_theory(pC_star, F, C) # --- relate qdot, u --- # the velocities of B^, C^ are zero since B, C are assumed to roll w/o slip kde = [dot(p.vel(F), A.y) for p in [pB_hat, pC_hat]] kde += [u1 - q1d] kde_map = solve(kde, [q1d, q2d, q3d]) for k, v in list(kde_map.items()): kde_map[k.diff(t)] = v.diff(t) # inertias of bodies A, B, C # IA22, IA23, IA33 are not specified in the problem statement, but are # necessary to define an inertia object. Although the values of # IA22, IA23, IA33 are not known in terms of the variables given in the # problem statement, they do not appear in the general inertia terms. inertia_A = inertia(A, IA, IA22, IA33, 0, IA23, 0) inertia_B = inertia(B, K, K, J) inertia_C = inertia(C, K, K, J) # define the rigid bodies A, B, C rbA = RigidBody('rbA', pA_star, A, mA, (inertia_A, pA_star)) rbB = RigidBody('rbB', pB_star, B, mB, (inertia_B, pB_star)) rbC = RigidBody('rbC', pC_star, C, mB, (inertia_C, pC_star)) ## --- use kanes method --- km = KanesMethod(F, [q1, q2, q3], [u1, u2], kd_eqs=kde, u_auxiliary=[u3]) forces = [(pS_star, -M * g * F.x), (pQ, Q1 * A.x + Q2 * A.y + Q3 * A.z)] bodies = [rbA, rbB, rbC] # Q2 = -u_prime * u2 * Q1 / sqrt(u2**2 + f**2 * u1**2) # -u_prime * R * u2 / sqrt(u2**2 + f**2 * u1**2) = R / Q1 * Q2 fr_expected = Matrix([ f * Q3 + M * g * e * sin(theta) * cos(q1), Q2 + M * g * sin(theta) * sin(q1), e * M * g * cos(theta) - Q1 * f - Q2 * R ]) #Q1 * (f - u_prime * R * u2 / sqrt(u2**2 + f**2 * u1**2)))]) fr_star_expected = Matrix([ -(IA + 2 * J * b**2 / R**2 + 2 * K + mA * a**2 + 2 * mB * b**2) * u1.diff(t) - mA * a * u1 * u2, -(mA + 2 * mB + 2 * J / R**2) * u2.diff(t) + mA * a * u1**2, 0 ]) with warns_deprecated_sympy(): fr, fr_star = km.kanes_equations(forces, bodies) assert (fr.expand() == fr_expected.expand()) assert ((fr_star_expected - trigsimp(fr_star)).expand() == zeros(3, 1))
def _form_frstar(self, bl): """Form the generalized inertia force.""" if not iterable(bl): raise TypeError('Bodies must be supplied in an iterable.') t = dynamicsymbols._t N = self._inertial # Dicts setting things to zero udot_zero = dict((i, 0) for i in self._udot) uaux_zero = dict((i, 0) for i in self._uaux) uauxdot = [diff(i, t) for i in self._uaux] uauxdot_zero = dict((i, 0) for i in uauxdot) # Dictionary of q' and q'' to u and u' q_ddot_u_map = dict( (k.diff(t), v.diff(t)) for (k, v) in self._qdot_u_map.items()) q_ddot_u_map.update(self._qdot_u_map) # Fill up the list of partials: format is a list with num 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. def get_partial_velocity(body): if isinstance(body, RigidBody): vlist = [body.masscenter.vel(N), body.frame.ang_vel_in(N)] elif isinstance(body, Particle): vlist = [ body.point.vel(N), ] else: raise TypeError('The body list may only contain either ' 'RigidBody or Particle as list elements.') v = [msubs(vel, self._qdot_u_map) for vel in vlist] return partial_velocity(v, self.u, N) partials = [get_partial_velocity(body) for body in bl] # Compute fr_star in two components: # fr_star = -(MM*u' + nonMM) o = len(self.u) MM = zeros(o, o) nonMM = zeros(o, 1) zero_uaux = lambda expr: msubs(expr, uaux_zero) zero_udot_uaux = lambda expr: msubs(msubs(expr, udot_zero), uaux_zero) for i, body in enumerate(bl): if isinstance(body, RigidBody): M = zero_uaux(body.mass) I = zero_uaux(body.central_inertia) vel = zero_uaux(body.masscenter.vel(N)) omega = zero_uaux(body.frame.ang_vel_in(N)) acc = zero_udot_uaux(body.masscenter.acc(N)) inertial_force = (M.diff(t) * vel + M * acc) inertial_torque = zero_uaux( (I.dt(body.frame) & omega) + msubs(I & body.frame.ang_acc_in(N), udot_zero) + (omega ^ (I & omega))) for j in range(o): tmp_vel = zero_uaux(partials[i][0][j]) tmp_ang = zero_uaux(I & partials[i][1][j]) 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] else: M = zero_uaux(body.mass) vel = zero_uaux(body.point.vel(N)) acc = zero_udot_uaux(body.point.acc(N)) inertial_force = (M.diff(t) * vel + M * acc) for j in range(o): temp = zero_uaux(partials[i][0][j]) for k in range(o): MM[j, k] += M * (temp & partials[i][0][k]) nonMM[j] += inertial_force & partials[i][0][j] # Compose fr_star out of MM and nonMM MM = zero_uaux(msubs(MM, q_ddot_u_map)) nonMM = msubs(msubs(nonMM, q_ddot_u_map), udot_zero, uauxdot_zero, uaux_zero) fr_star = -(MM * msubs(Matrix(self._udot), uauxdot_zero) + nonMM) # If there are dependent speeds, we need to find fr_star_tilde if self._udep: p = o - len(self._udep) fr_star_ind = fr_star[:p, 0] fr_star_dep = fr_star[p:o, 0] fr_star = fr_star_ind + (self._Ars.T * fr_star_dep) # Apply the same to MM MMi = MM[:p, :] MMd = MM[p:o, :] MM = MMi + (self._Ars.T * MMd) self._bodylist = bl self._frstar = fr_star self._k_d = MM self._f_d = -msubs(self._fr + self._frstar, udot_zero) return fr_star
def __init__(self, name, indices=None, latexs=None, variables=None): """ReferenceFrame initialization method. A ReferenceFrame has a set of orthonormal basis vectors, along with orientations relative to other ReferenceFrames and angular velocities relative to other ReferenceFrames. Parameters ========== indices : tuple of str Enables the reference frame's basis unit vectors to be accessed by Python's square bracket indexing notation using the provided three indice strings and alters the printing of the unit vectors to reflect this choice. latexs : tuple of str Alters the LaTeX printing of the reference frame's basis unit vectors to the provided three valid LaTeX strings. Examples ======== >>> from sympy.physics.vector import ReferenceFrame, vlatex >>> N = ReferenceFrame('N') >>> N.x N.x >>> O = ReferenceFrame('O', indices=('1', '2', '3')) >>> O.x O['1'] >>> O['1'] O['1'] >>> P = ReferenceFrame('P', latexs=('A1', 'A2', 'A3')) >>> vlatex(P.x) 'A1' symbols() can be used to create multiple Reference Frames in one step, for example: >>> from sympy.physics.vector import ReferenceFrame >>> from sympy import symbols >>> A, B, C = symbols('A B C', cls=ReferenceFrame) >>> D, E = symbols('D E', cls=ReferenceFrame, indices=('1', '2', '3')) >>> A[0] A_x >>> D.x D['1'] >>> E.y E['2'] >>> type(A) == type(D) True """ if not isinstance(name, str): raise TypeError('Need to supply a valid name') # The if statements below are for custom printing of basis-vectors for # each frame. # First case, when custom indices are supplied if indices is not None: if not isinstance(indices, (tuple, list)): raise TypeError('Supply the indices as a list') if len(indices) != 3: raise ValueError('Supply 3 indices') for i in indices: if not isinstance(i, str): raise TypeError('Indices must be strings') self.str_vecs = [(name + '[\'' + indices[0] + '\']'), (name + '[\'' + indices[1] + '\']'), (name + '[\'' + indices[2] + '\']')] self.pretty_vecs = [(name.lower() + u"_" + indices[0]), (name.lower() + u"_" + indices[1]), (name.lower() + u"_" + indices[2])] self.latex_vecs = [(r"\mathbf{\hat{%s}_{%s}}" % (name.lower(), indices[0])), (r"\mathbf{\hat{%s}_{%s}}" % (name.lower(), indices[1])), (r"\mathbf{\hat{%s}_{%s}}" % (name.lower(), indices[2]))] self.indices = indices # Second case, when no custom indices are supplied else: self.str_vecs = [(name + '.x'), (name + '.y'), (name + '.z')] self.pretty_vecs = [name.lower() + u"_x", name.lower() + u"_y", name.lower() + u"_z"] self.latex_vecs = [(r"\mathbf{\hat{%s}_x}" % name.lower()), (r"\mathbf{\hat{%s}_y}" % name.lower()), (r"\mathbf{\hat{%s}_z}" % name.lower())] self.indices = ['x', 'y', 'z'] # Different step, for custom latex basis vectors if latexs is not None: if not isinstance(latexs, (tuple, list)): raise TypeError('Supply the indices as a list') if len(latexs) != 3: raise ValueError('Supply 3 indices') for i in latexs: if not isinstance(i, str): raise TypeError('Latex entries must be strings') self.latex_vecs = latexs self.name = name self._var_dict = {} #The _dcm_dict dictionary will only store the dcms of parent-child #relationships. The _dcm_cache dictionary will work as the dcm #cache. self._dcm_dict = {} self._dcm_cache = {} self._ang_vel_dict = {} self._ang_acc_dict = {} self._dlist = [self._dcm_dict, self._ang_vel_dict, self._ang_acc_dict] self._cur = 0 self._x = Vector([(Matrix([1, 0, 0]), self)]) self._y = Vector([(Matrix([0, 1, 0]), self)]) self._z = Vector([(Matrix([0, 0, 1]), self)]) #Associate coordinate symbols wrt this frame if variables is not None: if not isinstance(variables, (tuple, list)): raise TypeError('Supply the variable names as a list/tuple') if len(variables) != 3: raise ValueError('Supply 3 variable names') for i in variables: if not isinstance(i, str): raise TypeError('Variable names must be strings') else: variables = [name + '_x', name + '_y', name + '_z'] self.varlist = (CoordinateSym(variables[0], self, 0), \ CoordinateSym(variables[1], self, 1), \ CoordinateSym(variables[2], self, 2)) ReferenceFrame._count += 1 self.index = ReferenceFrame._count
def __init__(self, Lagrangian, qs, forcelist=None, bodies=None, frame=None, hol_coneqs=None, nonhol_coneqs=None): """Supply the following for the initialization of LagrangesMethod Lagrangian : Sympifyable qs : array_like The generalized coordinates hol_coneqs : array_like, optional The holonomic constraint equations nonhol_coneqs : array_like, optional The nonholonomic constraint equations forcelist : iterable, optional Takes an iterable of (Point, Vector) or (ReferenceFrame, Vector) tuples which represent the force at a point or torque on a frame. This feature is primarily to account for the nonconservative forces and/or moments. bodies : iterable, optional Takes an iterable containing the rigid bodies and particles of the system. frame : ReferenceFrame, optional Supply the inertial frame. This is used to determine the generalized forces due to non-conservative forces. """ self._L = Matrix([sympify(Lagrangian)]) self.eom = None self._m_cd = Matrix() # Mass Matrix of differentiated coneqs self._m_d = Matrix() # Mass Matrix of dynamic equations self._f_cd = Matrix() # Forcing part of the diff coneqs self._f_d = Matrix() # Forcing part of the dynamic equations self.lam_coeffs = Matrix() # The coeffecients of the multipliers forcelist = forcelist if forcelist else [] if not iterable(forcelist): raise TypeError('Force pairs must be supplied in an iterable.') self._forcelist = forcelist if frame and not isinstance(frame, ReferenceFrame): raise TypeError('frame must be a valid ReferenceFrame') self._bodies = bodies self.inertial = frame self.lam_vec = Matrix() self._term1 = Matrix() self._term2 = Matrix() self._term3 = Matrix() self._term4 = Matrix() # Creating the qs, qdots and qdoubledots if not iterable(qs): raise TypeError('Generalized coordinates must be an iterable') self._q = Matrix(qs) self._qdots = self.q.diff(dynamicsymbols._t) self._qdoubledots = self._qdots.diff(dynamicsymbols._t) mat_build = lambda x: Matrix(x) if x else Matrix() hol_coneqs = mat_build(hol_coneqs) nonhol_coneqs = mat_build(nonhol_coneqs) self.coneqs = Matrix([hol_coneqs.diff(dynamicsymbols._t), nonhol_coneqs]) self._hol_coneqs = hol_coneqs
def test_linearize_pendulum_kane_nonminimal(): # Create generalized coordinates and speeds for this non-minimal realization # q1, q2 = N.x and N.y coordinates of pendulum # u1, u2 = N.x and N.y velocities of pendulum q1, q2 = dynamicsymbols('q1:3') q1d, q2d = dynamicsymbols('q1:3', level=1) u1, u2 = dynamicsymbols('u1:3') u1d, u2d = dynamicsymbols('u1:3', level=1) L, m, t = symbols('L, m, t') g = 9.8 # Compose world frame N = ReferenceFrame('N') pN = Point('N*') pN.set_vel(N, 0) # A.x is along the pendulum theta1 = atan(q2 / q1) A = N.orientnew('A', 'axis', [theta1, N.z]) # Locate the pendulum mass P = pN.locatenew('P1', q1 * N.x + q2 * N.y) pP = Particle('pP', P, m) # Calculate the kinematic differential equations kde = Matrix([q1d - u1, q2d - u2]) dq_dict = solve(kde, [q1d, q2d]) # Set velocity of point P P.set_vel(N, P.pos_from(pN).dt(N).subs(dq_dict)) # Configuration constraint is length of pendulum f_c = Matrix([P.pos_from(pN).magnitude() - L]) # Velocity constraint is that the velocity in the A.x direction is # always zero (the pendulum is never getting longer). f_v = Matrix([P.vel(N).express(A).dot(A.x)]) f_v.simplify() # Acceleration constraints is the time derivative of the velocity constraint f_a = f_v.diff(t) f_a.simplify() # Input the force resultant at P R = m * g * N.x # Derive the equations of motion using the KanesMethod class. KM = KanesMethod(N, q_ind=[q2], u_ind=[u2], q_dependent=[q1], u_dependent=[u1], configuration_constraints=f_c, velocity_constraints=f_v, acceleration_constraints=f_a, kd_eqs=kde) with warnings.catch_warnings(): warnings.filterwarnings("ignore", category=SymPyDeprecationWarning) (fr, frstar) = KM.kanes_equations([(P, R)], [pP]) # Set the operating point to be straight down, and non-moving q_op = {q1: L, q2: 0} u_op = {u1: 0, u2: 0} ud_op = {u1d: 0, u2d: 0} A, B, inp_vec = KM.linearize(op_point=[q_op, u_op, ud_op], A_and_B=True, simplify=True) assert A.expand() == Matrix([[0, 1], [-9.8 / L, 0]]) assert B == Matrix([])
def test_sub_qdot2(): # This test solves exercises 8.3 from Kane 1985 and defines # all velocities in terms of q, qdot. We check that the generalized active # forces are correctly computed if u terms are only defined in the # kinematic differential equations. # # This functionality was added in PR 8948. Without qdot/u substitution, the # KanesMethod constructor will fail during the constraint initialization as # the B matrix will be poorly formed and inversion of the dependent part # will fail. g, m, Px, Py, Pz, R, t = symbols('g m Px Py Pz R t') q = dynamicsymbols('q:5') qd = dynamicsymbols('q:5', level=1) u = dynamicsymbols('u:5') ## Define inertial, intermediate, and rigid body reference frames A = ReferenceFrame('A') B_prime = A.orientnew('B_prime', 'Axis', [q[0], A.z]) B = B_prime.orientnew('B', 'Axis', [pi / 2 - q[1], B_prime.x]) C = B.orientnew('C', 'Axis', [q[2], B.z]) ## Define points of interest and their velocities pO = Point('O') pO.set_vel(A, 0) # R is the point in plane H that comes into contact with disk C. pR = pO.locatenew('R', q[3] * A.x + q[4] * A.y) pR.set_vel(A, pR.pos_from(pO).diff(t, A)) pR.set_vel(B, 0) # C^ is the point in disk C that comes into contact with plane H. pC_hat = pR.locatenew('C^', 0) pC_hat.set_vel(C, 0) # C* is the point at the center of disk C. pCs = pC_hat.locatenew('C*', R * B.y) pCs.set_vel(C, 0) pCs.set_vel(B, 0) # calculate velocites of points C* and C^ in frame A pCs.v2pt_theory(pR, A, B) # points C* and R are fixed in frame B pC_hat.v2pt_theory(pCs, A, C) # points C* and C^ are fixed in frame C ## Define forces on each point of the system R_C_hat = Px * A.x + Py * A.y + Pz * A.z R_Cs = -m * g * A.z forces = [(pC_hat, R_C_hat), (pCs, R_Cs)] ## Define kinematic differential equations # let ui = omega_C_A & bi (i = 1, 2, 3) # u4 = qd4, u5 = qd5 u_expr = [C.ang_vel_in(A) & uv for uv in B] u_expr += qd[3:] kde = [ui - e for ui, e in zip(u, u_expr)] km1 = KanesMethod(A, q, u, kde) with warns_deprecated_sympy(): fr1, _ = km1.kanes_equations(forces, []) ## Calculate generalized active forces if we impose the condition that the # disk C is rolling without slipping u_indep = u[:3] u_dep = list(set(u) - set(u_indep)) vc = [pC_hat.vel(A) & uv for uv in [A.x, A.y]] km2 = KanesMethod(A, q, u_indep, kde, u_dependent=u_dep, velocity_constraints=vc) with warns_deprecated_sympy(): fr2, _ = km2.kanes_equations(forces, []) fr1_expected = Matrix([ -R * g * m * sin(q[1]), -R * (Px * cos(q[0]) + Py * sin(q[0])) * tan(q[1]), R * (Px * cos(q[0]) + Py * sin(q[0])), Px, Py ]) fr2_expected = Matrix([-R * g * m * sin(q[1]), 0, 0]) assert (trigsimp(fr1.expand()) == trigsimp(fr1_expected.expand())) assert (trigsimp(fr2.expand()) == trigsimp(fr2_expected.expand()))
def test_linearize_rolling_disc_kane(): # Symbols for time and constant parameters t, r, m, g, v = symbols('t r m g v') # Configuration variables and their time derivatives q1, q2, q3, q4, q5, q6 = q = dynamicsymbols('q1:7') q1d, q2d, q3d, q4d, q5d, q6d = qd = [qi.diff(t) for qi in q] # Generalized speeds and their time derivatives u = dynamicsymbols('u:6') u1, u2, u3, u4, u5, u6 = u = dynamicsymbols('u1:7') u1d, u2d, u3d, u4d, u5d, u6d = [ui.diff(t) for ui in u] # Reference frames N = ReferenceFrame('N') # Inertial frame NO = Point('NO') # Inertial origin A = N.orientnew('A', 'Axis', [q1, N.z]) # Yaw intermediate frame B = A.orientnew('B', 'Axis', [q2, A.x]) # Lean intermediate frame C = B.orientnew('C', 'Axis', [q3, B.y]) # Disc fixed frame CO = NO.locatenew('CO', q4*N.x + q5*N.y + q6*N.z) # Disc center # Disc angular velocity in N expressed using time derivatives of coordinates w_c_n_qd = C.ang_vel_in(N) w_b_n_qd = B.ang_vel_in(N) # Inertial angular velocity and angular acceleration of disc fixed frame C.set_ang_vel(N, u1*B.x + u2*B.y + u3*B.z) # Disc center velocity in N expressed using time derivatives of coordinates v_co_n_qd = CO.pos_from(NO).dt(N) # Disc center velocity in N expressed using generalized speeds CO.set_vel(N, u4*C.x + u5*C.y + u6*C.z) # Disc Ground Contact Point P = CO.locatenew('P', r*B.z) P.v2pt_theory(CO, N, C) # Configuration constraint f_c = Matrix([q6 - dot(CO.pos_from(P), N.z)]) # Velocity level constraints f_v = Matrix([dot(P.vel(N), uv) for uv in C]) # Kinematic differential equations kindiffs = Matrix([dot(w_c_n_qd - C.ang_vel_in(N), uv) for uv in B] + [dot(v_co_n_qd - CO.vel(N), uv) for uv in N]) qdots = solve(kindiffs, qd) # Set angular velocity of remaining frames B.set_ang_vel(N, w_b_n_qd.subs(qdots)) C.set_ang_acc(N, C.ang_vel_in(N).dt(B) + cross(B.ang_vel_in(N), C.ang_vel_in(N))) # Active forces F_CO = m*g*A.z # Create inertia dyadic of disc C about point CO I = (m * r**2) / 4 J = (m * r**2) / 2 I_C_CO = inertia(C, I, J, I) Disc = RigidBody('Disc', CO, C, m, (I_C_CO, CO)) BL = [Disc] FL = [(CO, F_CO)] KM = KanesMethod(N, [q1, q2, q3, q4, q5], [u1, u2, u3], kd_eqs=kindiffs, q_dependent=[q6], configuration_constraints=f_c, u_dependent=[u4, u5, u6], velocity_constraints=f_v) with warnings.catch_warnings(): warnings.filterwarnings("ignore", category=SymPyDeprecationWarning) (fr, fr_star) = KM.kanes_equations(FL, BL) # Test generalized form equations linearizer = KM.to_linearizer() assert linearizer.f_c == f_c assert linearizer.f_v == f_v assert linearizer.f_a == f_v.diff(t) sol = solve(linearizer.f_0 + linearizer.f_1, qd) for qi in qd: assert sol[qi] == qdots[qi] assert simplify(linearizer.f_2 + linearizer.f_3 - fr - fr_star) == Matrix([0, 0, 0]) # Perform the linearization # Precomputed operating point q_op = {q6: -r*cos(q2)} u_op = {u1: 0, u2: sin(q2)*q1d + q3d, u3: cos(q2)*q1d, u4: -r*(sin(q2)*q1d + q3d)*cos(q3), u5: 0, u6: -r*(sin(q2)*q1d + q3d)*sin(q3)} qd_op = {q2d: 0, q4d: -r*(sin(q2)*q1d + q3d)*cos(q1), q5d: -r*(sin(q2)*q1d + q3d)*sin(q1), q6d: 0} ud_op = {u1d: 4*g*sin(q2)/(5*r) + sin(2*q2)*q1d**2/2 + 6*cos(q2)*q1d*q3d/5, u2d: 0, u3d: 0, u4d: r*(sin(q2)*sin(q3)*q1d*q3d + sin(q3)*q3d**2), u5d: r*(4*g*sin(q2)/(5*r) + sin(2*q2)*q1d**2/2 + 6*cos(q2)*q1d*q3d/5), u6d: -r*(sin(q2)*cos(q3)*q1d*q3d + cos(q3)*q3d**2)} A, B = linearizer.linearize(op_point=[q_op, u_op, qd_op, ud_op], A_and_B=True, simplify=True) upright_nominal = {q1d: 0, q2: 0, m: 1, r: 1, g: 1} # Precomputed solution A_sol = Matrix([[0, 0, 0, 0, 0, 0, 0, 1], [0, 0, 0, 0, 0, 1, 0, 0], [0, 0, 0, 0, 0, 0, 1, 0], [sin(q1)*q3d, 0, 0, 0, 0, -sin(q1), -cos(q1), 0], [-cos(q1)*q3d, 0, 0, 0, 0, cos(q1), -sin(q1), 0], [0, 4/5, 0, 0, 0, 0, 0, 6*q3d/5], [0, 0, 0, 0, 0, 0, 0, 0], [0, 0, 0, 0, 0, -2*q3d, 0, 0]]) B_sol = Matrix([]) # Check that linearization is correct assert A.subs(upright_nominal) == A_sol assert B.subs(upright_nominal) == B_sol # Check eigenvalues at critical speed are all zero: assert A.subs(upright_nominal).subs(q3d, 1/sqrt(3)).eigenvals() == {0: 8}
def test_linearize_rolling_disc_kane(): # Symbols for time and constant parameters t, r, m, g, v = symbols('t r m g v') # Configuration variables and their time derivatives q1, q2, q3, q4, q5, q6 = q = dynamicsymbols('q1:7') q1d, q2d, q3d, q4d, q5d, q6d = qd = [qi.diff(t) for qi in q] # Generalized speeds and their time derivatives u = dynamicsymbols('u:6') u1, u2, u3, u4, u5, u6 = u = dynamicsymbols('u1:7') u1d, u2d, u3d, u4d, u5d, u6d = [ui.diff(t) for ui in u] # Reference frames N = ReferenceFrame('N') # Inertial frame NO = Point('NO') # Inertial origin A = N.orientnew('A', 'Axis', [q1, N.z]) # Yaw intermediate frame B = A.orientnew('B', 'Axis', [q2, A.x]) # Lean intermediate frame C = B.orientnew('C', 'Axis', [q3, B.y]) # Disc fixed frame CO = NO.locatenew('CO', q4 * N.x + q5 * N.y + q6 * N.z) # Disc center # Disc angular velocity in N expressed using time derivatives of coordinates w_c_n_qd = C.ang_vel_in(N) w_b_n_qd = B.ang_vel_in(N) # Inertial angular velocity and angular acceleration of disc fixed frame C.set_ang_vel(N, u1 * B.x + u2 * B.y + u3 * B.z) # Disc center velocity in N expressed using time derivatives of coordinates v_co_n_qd = CO.pos_from(NO).dt(N) # Disc center velocity in N expressed using generalized speeds CO.set_vel(N, u4 * C.x + u5 * C.y + u6 * C.z) # Disc Ground Contact Point P = CO.locatenew('P', r * B.z) P.v2pt_theory(CO, N, C) # Configuration constraint f_c = Matrix([q6 - dot(CO.pos_from(P), N.z)]) # Velocity level constraints f_v = Matrix([dot(P.vel(N), uv) for uv in C]) # Kinematic differential equations kindiffs = Matrix([dot(w_c_n_qd - C.ang_vel_in(N), uv) for uv in B] + [dot(v_co_n_qd - CO.vel(N), uv) for uv in N]) qdots = solve(kindiffs, qd) # Set angular velocity of remaining frames B.set_ang_vel(N, w_b_n_qd.subs(qdots)) C.set_ang_acc( N, C.ang_vel_in(N).dt(B) + cross(B.ang_vel_in(N), C.ang_vel_in(N))) # Active forces F_CO = m * g * A.z # Create inertia dyadic of disc C about point CO I = (m * r**2) / 4 J = (m * r**2) / 2 I_C_CO = inertia(C, I, J, I) Disc = RigidBody('Disc', CO, C, m, (I_C_CO, CO)) BL = [Disc] FL = [(CO, F_CO)] KM = KanesMethod(N, [q1, q2, q3, q4, q5], [u1, u2, u3], kd_eqs=kindiffs, q_dependent=[q6], configuration_constraints=f_c, u_dependent=[u4, u5, u6], velocity_constraints=f_v) with warnings.catch_warnings(): warnings.filterwarnings("ignore", category=SymPyDeprecationWarning) (fr, fr_star) = KM.kanes_equations(FL, BL) # Test generalized form equations linearizer = KM.to_linearizer() assert linearizer.f_c == f_c assert linearizer.f_v == f_v assert linearizer.f_a == f_v.diff(t) sol = solve(linearizer.f_0 + linearizer.f_1, qd) for qi in qd: assert sol[qi] == qdots[qi] assert simplify(linearizer.f_2 + linearizer.f_3 - fr - fr_star) == Matrix( [0, 0, 0]) # Perform the linearization # Precomputed operating point q_op = {q6: -r * cos(q2)} u_op = { u1: 0, u2: sin(q2) * q1d + q3d, u3: cos(q2) * q1d, u4: -r * (sin(q2) * q1d + q3d) * cos(q3), u5: 0, u6: -r * (sin(q2) * q1d + q3d) * sin(q3) } qd_op = { q2d: 0, q4d: -r * (sin(q2) * q1d + q3d) * cos(q1), q5d: -r * (sin(q2) * q1d + q3d) * sin(q1), q6d: 0 } ud_op = { u1d: 4 * g * sin(q2) / (5 * r) + sin(2 * q2) * q1d**2 / 2 + 6 * cos(q2) * q1d * q3d / 5, u2d: 0, u3d: 0, u4d: r * (sin(q2) * sin(q3) * q1d * q3d + sin(q3) * q3d**2), u5d: r * (4 * g * sin(q2) / (5 * r) + sin(2 * q2) * q1d**2 / 2 + 6 * cos(q2) * q1d * q3d / 5), u6d: -r * (sin(q2) * cos(q3) * q1d * q3d + cos(q3) * q3d**2) } A, B = linearizer.linearize(op_point=[q_op, u_op, qd_op, ud_op], A_and_B=True, simplify=True) upright_nominal = {q1d: 0, q2: 0, m: 1, r: 1, g: 1} # Precomputed solution A_sol = Matrix([[0, 0, 0, 0, 0, 0, 0, 1], [0, 0, 0, 0, 0, 1, 0, 0], [0, 0, 0, 0, 0, 0, 1, 0], [sin(q1) * q3d, 0, 0, 0, 0, -sin(q1), -cos(q1), 0], [-cos(q1) * q3d, 0, 0, 0, 0, cos(q1), -sin(q1), 0], [0, 4 / 5, 0, 0, 0, 0, 0, 6 * q3d / 5], [0, 0, 0, 0, 0, 0, 0, 0], [0, 0, 0, 0, 0, -2 * q3d, 0, 0]]) B_sol = Matrix([]) # Check that linearization is correct assert A.subs(upright_nominal) == A_sol assert B.subs(upright_nominal) == B_sol # Check eigenvalues at critical speed are all zero: assert A.subs(upright_nominal).subs(q3d, 1 / sqrt(3)).eigenvals() == {0: 8}
# This class is going to be tested using a simple pendulum set up in x and y # coordinates x, y, u, v, lam = dynamicsymbols('x y u v lambda') m, l, g = symbols('m l g') # Set up the different forms the equations can take # [1] Explicit form where the kinematics and dynamics are combined # x' = F(x, t, r, p) # # [2] Implicit form where the kinematics and dynamics are combined # M(x, p) x' = F(x, t, r, p) # # [3] Implicit form where the kinematics and dynamics are separate # M(q, p) u' = F(q, u, t, r, p) # q' = G(q, u, t, r, p) dyn_implicit_mat = Matrix([[1, 0, -x / m], [0, 1, -y / m], [0, 0, l**2 / m]]) dyn_implicit_rhs = Matrix([0, 0, u**2 + v**2 - g * y]) comb_implicit_mat = Matrix([[1, 0, 0, 0, 0], [0, 1, 0, 0, 0], [0, 0, 1, 0, -x / m], [0, 0, 0, 1, -y / m], [0, 0, 0, 0, l**2 / m]]) comb_implicit_rhs = Matrix([u, v, 0, 0, u**2 + v**2 - g * y]) kin_explicit_rhs = Matrix([u, v]) comb_explicit_rhs = comb_implicit_mat.LUsolve(comb_implicit_rhs) # Set up a body and load to pass into the system theta = atan(x / y)
class Linearizer(object): """This object holds the general model form for a dynamic system. This model is used for computing the linearized form of the system, while properly dealing with constraints leading to dependent coordinates and speeds. Attributes ---------- f_0, f_1, f_2, f_3, f_4, f_c, f_v, f_a : Matrix Matrices holding the general system form. q, u, r : Matrix Matrices holding the generalized coordinates, speeds, and input vectors. q_i, u_i : Matrix Matrices of the independent generalized coordinates and speeds. q_d, u_d : Matrix Matrices of the dependent generalized coordinates and speeds. perm_mat : Matrix Permutation matrix such that [q_ind, u_ind]^T = perm_mat*[q, u]^T """ def __init__(self, f_0, f_1, f_2, f_3, f_4, f_c, f_v, f_a, q, u, q_i=None, q_d=None, u_i=None, u_d=None, r=None, lams=None): """ Parameters ---------- f_0, f_1, f_2, f_3, f_4, f_c, f_v, f_a : array_like System of equations holding the general system form. Supply empty array or Matrix if the parameter doesn't exist. q : array_like The generalized coordinates. u : array_like The generalized speeds q_i, u_i : array_like, optional The independent generalized coordinates and speeds. q_d, u_d : array_like, optional The dependent generalized coordinates and speeds. r : array_like, optional The input variables. lams : array_like, optional The lagrange multipliers """ # Generalized equation form self.f_0 = Matrix(f_0) self.f_1 = Matrix(f_1) self.f_2 = Matrix(f_2) self.f_3 = Matrix(f_3) self.f_4 = Matrix(f_4) self.f_c = Matrix(f_c) self.f_v = Matrix(f_v) self.f_a = Matrix(f_a) # Generalized equation variables self.q = Matrix(q) self.u = Matrix(u) none_handler = lambda x: Matrix(x) if x else Matrix() self.q_i = none_handler(q_i) self.q_d = none_handler(q_d) self.u_i = none_handler(u_i) self.u_d = none_handler(u_d) self.r = none_handler(r) self.lams = none_handler(lams) # Derivatives of generalized equation variables self._qd = self.q.diff(dynamicsymbols._t) self._ud = self.u.diff(dynamicsymbols._t) # If the user doesn't actually use generalized variables, and the # qd and u vectors have any intersecting variables, this can cause # problems. We'll fix this with some hackery, and Dummy variables dup_vars = set(self._qd).intersection(self.u) self._qd_dup = Matrix( [var if var not in dup_vars else Dummy() for var in self._qd]) # Derive dimesion terms l = len(self.f_c) m = len(self.f_v) n = len(self.q) o = len(self.u) s = len(self.r) k = len(self.lams) dims = namedtuple('dims', ['l', 'm', 'n', 'o', 's', 'k']) self._dims = dims(l, m, n, o, s, k) self._setup_done = False def _setup(self): # Calculations here only need to be run once. They are moved out of # the __init__ method to increase the speed of Linearizer creation. self._form_permutation_matrices() self._form_block_matrices() self._form_coefficient_matrices() self._setup_done = True def _form_permutation_matrices(self): """Form the permutation matrices Pq and Pu.""" # Extract dimension variables l, m, n, o, s, k = self._dims # Compute permutation matrices if n != 0: self._Pq = permutation_matrix(self.q, Matrix([self.q_i, self.q_d])) if l > 0: self._Pqi = self._Pq[:, :-l] self._Pqd = self._Pq[:, -l:] else: self._Pqi = self._Pq self._Pqd = Matrix() if o != 0: self._Pu = permutation_matrix(self.u, Matrix([self.u_i, self.u_d])) if m > 0: self._Pui = self._Pu[:, :-m] self._Pud = self._Pu[:, -m:] else: self._Pui = self._Pu self._Pud = Matrix() # Compute combination permutation matrix for computing A and B P_col1 = Matrix([self._Pqi, zeros(o + k, n - l)]) P_col2 = Matrix([zeros(n, o - m), self._Pui, zeros(k, o - m)]) if P_col1: if P_col2: self.perm_mat = P_col1.row_join(P_col2) else: self.perm_mat = P_col1 else: self.perm_mat = P_col2 def _form_coefficient_matrices(self): """Form the coefficient matrices C_0, C_1, and C_2.""" # Extract dimension variables l, m, n, o, s, k = self._dims # Build up the coefficient matrices C_0, C_1, and C_2 # If there are configuration constraints (l > 0), form C_0 as normal. # If not, C_0 is I_(nxn). Note that this works even if n=0 if l > 0: f_c_jac_q = self.f_c.jacobian(self.q) self._C_0 = ( eye(n) - self._Pqd * (f_c_jac_q * self._Pqd).LUsolve(f_c_jac_q)) * self._Pqi else: self._C_0 = eye(n) # If there are motion constraints (m > 0), form C_1 and C_2 as normal. # If not, C_1 is 0, and C_2 is I_(oxo). Note that this works even if # o = 0. if m > 0: f_v_jac_u = self.f_v.jacobian(self.u) temp = f_v_jac_u * self._Pud if n != 0: f_v_jac_q = self.f_v.jacobian(self.q) self._C_1 = -self._Pud * temp.LUsolve(f_v_jac_q) else: self._C_1 = zeros(o, n) self._C_2 = (eye(o) - self._Pud * temp.LUsolve(f_v_jac_u)) * self._Pui else: self._C_1 = zeros(o, n) self._C_2 = eye(o) def _form_block_matrices(self): """Form the block matrices for composing M, A, and B.""" # Extract dimension variables l, m, n, o, s, k = self._dims # Block Matrix Definitions. These are only defined if under certain # conditions. If undefined, an empty matrix is used instead if n != 0: self._M_qq = self.f_0.jacobian(self._qd) self._A_qq = -(self.f_0 + self.f_1).jacobian(self.q) else: self._M_qq = Matrix() self._A_qq = Matrix() if n != 0 and m != 0: self._M_uqc = self.f_a.jacobian(self._qd_dup) self._A_uqc = -self.f_a.jacobian(self.q) else: self._M_uqc = Matrix() self._A_uqc = Matrix() if n != 0 and o - m + k != 0: self._M_uqd = self.f_3.jacobian(self._qd_dup) self._A_uqd = -(self.f_2 + self.f_3 + self.f_4).jacobian(self.q) else: self._M_uqd = Matrix() self._A_uqd = Matrix() if o != 0 and m != 0: self._M_uuc = self.f_a.jacobian(self._ud) self._A_uuc = -self.f_a.jacobian(self.u) else: self._M_uuc = Matrix() self._A_uuc = Matrix() if o != 0 and o - m + k != 0: self._M_uud = self.f_2.jacobian(self._ud) self._A_uud = -(self.f_2 + self.f_3).jacobian(self.u) else: self._M_uud = Matrix() self._A_uud = Matrix() if o != 0 and n != 0: self._A_qu = -self.f_1.jacobian(self.u) else: self._A_qu = Matrix() if k != 0 and o - m + k != 0: self._M_uld = self.f_4.jacobian(self.lams) else: self._M_uld = Matrix() if s != 0 and o - m + k != 0: self._B_u = -self.f_3.jacobian(self.r) else: self._B_u = Matrix() def linearize(self, op_point=None, A_and_B=False, simplify=False): """Linearize the system about the operating point. Note that q_op, u_op, qd_op, ud_op must satisfy the equations of motion. These may be either symbolic or numeric. Parameters ---------- op_point : dict or iterable of dicts, optional Dictionary or iterable of dictionaries containing the operating point conditions. These will be substituted in to the linearized system before the linearization is complete. Leave blank if you want a completely symbolic form. Note that any reduction in symbols (whether substituted for numbers or expressions with a common parameter) will result in faster runtime. A_and_B : bool, optional If A_and_B=False (default), (M, A, B) is returned for forming [M]*[q, u]^T = [A]*[q_ind, u_ind]^T + [B]r. If A_and_B=True, (A, B) is returned for forming dx = [A]x + [B]r, where x = [q_ind, u_ind]^T. simplify : bool, optional Determines if returned values are simplified before return. For large expressions this may be time consuming. Default is False. Potential Issues ---------------- Note that the process of solving with A_and_B=True is computationally intensive if there are many symbolic parameters. For this reason, it may be more desirable to use the default A_and_B=False, returning M, A, and B. More values may then be substituted in to these matrices later on. The state space form can then be found as A = P.T*M.LUsolve(A), B = P.T*M.LUsolve(B), where P = Linearizer.perm_mat. """ # Run the setup if needed: if not self._setup_done: self._setup() # Compose dict of operating conditions if isinstance(op_point, dict): op_point_dict = op_point elif isinstance(op_point, Iterable): op_point_dict = {} for op in op_point: op_point_dict.update(op) else: op_point_dict = {} # Extract dimension variables l, m, n, o, s, k = self._dims # Rename terms to shorten expressions M_qq = self._M_qq M_uqc = self._M_uqc M_uqd = self._M_uqd M_uuc = self._M_uuc M_uud = self._M_uud M_uld = self._M_uld A_qq = self._A_qq A_uqc = self._A_uqc A_uqd = self._A_uqd A_qu = self._A_qu A_uuc = self._A_uuc A_uud = self._A_uud B_u = self._B_u C_0 = self._C_0 C_1 = self._C_1 C_2 = self._C_2 # Build up Mass Matrix # |M_qq 0_nxo 0_nxk| # M = |M_uqc M_uuc 0_mxk| # |M_uqd M_uud M_uld| if o != 0: col2 = Matrix([zeros(n, o), M_uuc, M_uud]) if k != 0: col3 = Matrix([zeros(n + m, k), M_uld]) if n != 0: col1 = Matrix([M_qq, M_uqc, M_uqd]) if o != 0 and k != 0: M = col1.row_join(col2).row_join(col3) elif o != 0: M = col1.row_join(col2) else: M = col1 elif k != 0: M = col2.row_join(col3) else: M = col2 M_eq = msubs(M, op_point_dict) # Build up state coefficient matrix A # |(A_qq + A_qu*C_1)*C_0 A_qu*C_2| # A = |(A_uqc + A_uuc*C_1)*C_0 A_uuc*C_2| # |(A_uqd + A_uud*C_1)*C_0 A_uud*C_2| # Col 1 is only defined if n != 0 if n != 0: r1c1 = A_qq if o != 0: r1c1 += (A_qu * C_1) r1c1 = r1c1 * C_0 if m != 0: r2c1 = A_uqc if o != 0: r2c1 += (A_uuc * C_1) r2c1 = r2c1 * C_0 else: r2c1 = Matrix() if o - m + k != 0: r3c1 = A_uqd if o != 0: r3c1 += (A_uud * C_1) r3c1 = r3c1 * C_0 else: r3c1 = Matrix() col1 = Matrix([r1c1, r2c1, r3c1]) else: col1 = Matrix() # Col 2 is only defined if o != 0 if o != 0: if n != 0: r1c2 = A_qu * C_2 else: r1c2 = Matrix() if m != 0: r2c2 = A_uuc * C_2 else: r2c2 = Matrix() if o - m + k != 0: r3c2 = A_uud * C_2 else: r3c2 = Matrix() col2 = Matrix([r1c2, r2c2, r3c2]) else: col2 = Matrix() if col1: if col2: Amat = col1.row_join(col2) else: Amat = col1 else: Amat = col2 Amat_eq = msubs(Amat, op_point_dict) # Build up the B matrix if there are forcing variables # |0_(n + m)xs| # B = |B_u | if s != 0 and o - m + k != 0: Bmat = zeros(n + m, s).col_join(B_u) Bmat_eq = msubs(Bmat, op_point_dict) else: Bmat_eq = Matrix() # kwarg A_and_B indicates to return A, B for forming the equation # dx = [A]x + [B]r, where x = [q_indnd, u_indnd]^T, if A_and_B: A_cont = self.perm_mat.T * M_eq.LUsolve(Amat_eq) if Bmat_eq: B_cont = self.perm_mat.T * M_eq.LUsolve(Bmat_eq) else: # Bmat = Matrix([]), so no need to sub B_cont = Bmat_eq if simplify: A_cont.simplify() B_cont.simplify() return A_cont, B_cont # Otherwise return M, A, B for forming the equation # [M]dx = [A]x + [B]r, where x = [q, u]^T else: if simplify: M_eq.simplify() Amat_eq.simplify() Bmat_eq.simplify() return M_eq, Amat_eq, Bmat_eq
class Linearizer(object): """This object holds the general model form for a dynamic system. This model is used for computing the linearized form of the system, while properly dealing with constraints leading to dependent coordinates and speeds. Attributes ---------- f_0, f_1, f_2, f_3, f_4, f_c, f_v, f_a : Matrix Matrices holding the general system form. q, u, r : Matrix Matrices holding the generalized coordinates, speeds, and input vectors. q_i, u_i : Matrix Matrices of the independent generalized coordinates and speeds. q_d, u_d : Matrix Matrices of the dependent generalized coordinates and speeds. perm_mat : Matrix Permutation matrix such that [q_ind, u_ind]^T = perm_mat*[q, u]^T """ def __init__(self, f_0, f_1, f_2, f_3, f_4, f_c, f_v, f_a, q, u, q_i=None, q_d=None, u_i=None, u_d=None, r=None, lams=None): """ Parameters ---------- f_0, f_1, f_2, f_3, f_4, f_c, f_v, f_a : array_like System of equations holding the general system form. Supply empty array or Matrix if the parameter doesn't exist. q : array_like The generalized coordinates. u : array_like The generalized speeds q_i, u_i : array_like, optional The independent generalized coordinates and speeds. q_d, u_d : array_like, optional The dependent generalized coordinates and speeds. r : array_like, optional The input variables. lams : array_like, optional The lagrange multipliers """ # Generalized equation form self.f_0 = Matrix(f_0) self.f_1 = Matrix(f_1) self.f_2 = Matrix(f_2) self.f_3 = Matrix(f_3) self.f_4 = Matrix(f_4) self.f_c = Matrix(f_c) self.f_v = Matrix(f_v) self.f_a = Matrix(f_a) # Generalized equation variables self.q = Matrix(q) self.u = Matrix(u) none_handler = lambda x: Matrix(x) if x else Matrix() self.q_i = none_handler(q_i) self.q_d = none_handler(q_d) self.u_i = none_handler(u_i) self.u_d = none_handler(u_d) self.r = none_handler(r) self.lams = none_handler(lams) # Derivatives of generalized equation variables self._qd = self.q.diff(dynamicsymbols._t) self._ud = self.u.diff(dynamicsymbols._t) # If the user doesn't actually use generalized variables, and the # qd and u vectors have any intersecting variables, this can cause # problems. We'll fix this with some hackery, and Dummy variables dup_vars = set(self._qd).intersection(self.u) self._qd_dup = Matrix([var if var not in dup_vars else Dummy() for var in self._qd]) # Derive dimesion terms l = len(self.f_c) m = len(self.f_v) n = len(self.q) o = len(self.u) s = len(self.r) k = len(self.lams) dims = namedtuple('dims', ['l', 'm', 'n', 'o', 's', 'k']) self._dims = dims(l, m, n, o, s, k) self._setup_done = False def _setup(self): # Calculations here only need to be run once. They are moved out of # the __init__ method to increase the speed of Linearizer creation. self._form_permutation_matrices() self._form_block_matrices() self._form_coefficient_matrices() self._setup_done = True def _form_permutation_matrices(self): """Form the permutation matrices Pq and Pu.""" # Extract dimension variables l, m, n, o, s, k = self._dims # Compute permutation matrices if n != 0: self._Pq = permutation_matrix(self.q, Matrix([self.q_i, self.q_d])) if l > 0: self._Pqi = self._Pq[:, :-l] self._Pqd = self._Pq[:, -l:] else: self._Pqi = self._Pq self._Pqd = Matrix() if o != 0: self._Pu = permutation_matrix(self.u, Matrix([self.u_i, self.u_d])) if m > 0: self._Pui = self._Pu[:, :-m] self._Pud = self._Pu[:, -m:] else: self._Pui = self._Pu self._Pud = Matrix() # Compute combination permutation matrix for computing A and B P_col1 = Matrix([self._Pqi, zeros(o + k, n - l)]) P_col2 = Matrix([zeros(n, o - m), self._Pui, zeros(k, o - m)]) if P_col1: if P_col2: self.perm_mat = P_col1.row_join(P_col2) else: self.perm_mat = P_col1 else: self.perm_mat = P_col2 def _form_coefficient_matrices(self): """Form the coefficient matrices C_0, C_1, and C_2.""" # Extract dimension variables l, m, n, o, s, k = self._dims # Build up the coefficient matrices C_0, C_1, and C_2 # If there are configuration constraints (l > 0), form C_0 as normal. # If not, C_0 is I_(nxn). Note that this works even if n=0 if l > 0: f_c_jac_q = self.f_c.jacobian(self.q) self._C_0 = (eye(n) - self._Pqd * (f_c_jac_q * self._Pqd).LUsolve(f_c_jac_q)) * self._Pqi else: self._C_0 = eye(n) # If there are motion constraints (m > 0), form C_1 and C_2 as normal. # If not, C_1 is 0, and C_2 is I_(oxo). Note that this works even if # o = 0. if m > 0: f_v_jac_u = self.f_v.jacobian(self.u) temp = f_v_jac_u * self._Pud if n != 0: f_v_jac_q = self.f_v.jacobian(self.q) self._C_1 = -self._Pud * temp.LUsolve(f_v_jac_q) else: self._C_1 = zeros(o, n) self._C_2 = (eye(o) - self._Pud * temp.LUsolve(f_v_jac_u)) * self._Pui else: self._C_1 = zeros(o, n) self._C_2 = eye(o) def _form_block_matrices(self): """Form the block matrices for composing M, A, and B.""" # Extract dimension variables l, m, n, o, s, k = self._dims # Block Matrix Definitions. These are only defined if under certain # conditions. If undefined, an empty matrix is used instead if n != 0: self._M_qq = self.f_0.jacobian(self._qd) self._A_qq = -(self.f_0 + self.f_1).jacobian(self.q) else: self._M_qq = Matrix() self._A_qq = Matrix() if n != 0 and m != 0: self._M_uqc = self.f_a.jacobian(self._qd_dup) self._A_uqc = -self.f_a.jacobian(self.q) else: self._M_uqc = Matrix() self._A_uqc = Matrix() if n != 0 and o - m + k != 0: self._M_uqd = self.f_3.jacobian(self._qd_dup) self._A_uqd = -(self.f_2 + self.f_3 + self.f_4).jacobian(self.q) else: self._M_uqd = Matrix() self._A_uqd = Matrix() if o != 0 and m != 0: self._M_uuc = self.f_a.jacobian(self._ud) self._A_uuc = -self.f_a.jacobian(self.u) else: self._M_uuc = Matrix() self._A_uuc = Matrix() if o != 0 and o - m + k != 0: self._M_uud = self.f_2.jacobian(self._ud) self._A_uud = -(self.f_2 + self.f_3).jacobian(self.u) else: self._M_uud = Matrix() self._A_uud = Matrix() if o != 0 and n != 0: self._A_qu = -self.f_1.jacobian(self.u) else: self._A_qu = Matrix() if k != 0 and o - m + k != 0: self._M_uld = self.f_4.jacobian(self.lams) else: self._M_uld = Matrix() if s != 0 and o - m + k != 0: self._B_u = -self.f_3.jacobian(self.r) else: self._B_u = Matrix() def linearize(self, op_point=None, A_and_B=False, simplify=False): """Linearize the system about the operating point. Note that q_op, u_op, qd_op, ud_op must satisfy the equations of motion. These may be either symbolic or numeric. Parameters ---------- op_point : dict or iterable of dicts, optional Dictionary or iterable of dictionaries containing the operating point conditions. These will be substituted in to the linearized system before the linearization is complete. Leave blank if you want a completely symbolic form. Note that any reduction in symbols (whether substituted for numbers or expressions with a common parameter) will result in faster runtime. A_and_B : bool, optional If A_and_B=False (default), (M, A, B) is returned for forming [M]*[q, u]^T = [A]*[q_ind, u_ind]^T + [B]r. If A_and_B=True, (A, B) is returned for forming dx = [A]x + [B]r, where x = [q_ind, u_ind]^T. simplify : bool, optional Determines if returned values are simplified before return. For large expressions this may be time consuming. Default is False. Potential Issues ---------------- Note that the process of solving with A_and_B=True is computationally intensive if there are many symbolic parameters. For this reason, it may be more desirable to use the default A_and_B=False, returning M, A, and B. More values may then be substituted in to these matrices later on. The state space form can then be found as A = P.T*M.LUsolve(A), B = P.T*M.LUsolve(B), where P = Linearizer.perm_mat. """ # Run the setup if needed: if not self._setup_done: self._setup() # Compose dict of operating conditions if isinstance(op_point, dict): op_point_dict = op_point elif isinstance(op_point, Iterable): op_point_dict = {} for op in op_point: op_point_dict.update(op) else: op_point_dict = {} # Extract dimension variables l, m, n, o, s, k = self._dims # Rename terms to shorten expressions M_qq = self._M_qq M_uqc = self._M_uqc M_uqd = self._M_uqd M_uuc = self._M_uuc M_uud = self._M_uud M_uld = self._M_uld A_qq = self._A_qq A_uqc = self._A_uqc A_uqd = self._A_uqd A_qu = self._A_qu A_uuc = self._A_uuc A_uud = self._A_uud B_u = self._B_u C_0 = self._C_0 C_1 = self._C_1 C_2 = self._C_2 # Build up Mass Matrix # |M_qq 0_nxo 0_nxk| # M = |M_uqc M_uuc 0_mxk| # |M_uqd M_uud M_uld| if o != 0: col2 = Matrix([zeros(n, o), M_uuc, M_uud]) if k != 0: col3 = Matrix([zeros(n + m, k), M_uld]) if n != 0: col1 = Matrix([M_qq, M_uqc, M_uqd]) if o != 0 and k != 0: M = col1.row_join(col2).row_join(col3) elif o != 0: M = col1.row_join(col2) else: M = col1 elif k != 0: M = col2.row_join(col3) else: M = col2 M_eq = msubs(M, op_point_dict) # Build up state coefficient matrix A # |(A_qq + A_qu*C_1)*C_0 A_qu*C_2| # A = |(A_uqc + A_uuc*C_1)*C_0 A_uuc*C_2| # |(A_uqd + A_uud*C_1)*C_0 A_uud*C_2| # Col 1 is only defined if n != 0 if n != 0: r1c1 = A_qq if o != 0: r1c1 += (A_qu * C_1) r1c1 = r1c1 * C_0 if m != 0: r2c1 = A_uqc if o != 0: r2c1 += (A_uuc * C_1) r2c1 = r2c1 * C_0 else: r2c1 = Matrix() if o - m + k != 0: r3c1 = A_uqd if o != 0: r3c1 += (A_uud * C_1) r3c1 = r3c1 * C_0 else: r3c1 = Matrix() col1 = Matrix([r1c1, r2c1, r3c1]) else: col1 = Matrix() # Col 2 is only defined if o != 0 if o != 0: if n != 0: r1c2 = A_qu * C_2 else: r1c2 = Matrix() if m != 0: r2c2 = A_uuc * C_2 else: r2c2 = Matrix() if o - m + k != 0: r3c2 = A_uud * C_2 else: r3c2 = Matrix() col2 = Matrix([r1c2, r2c2, r3c2]) else: col2 = Matrix() if col1: if col2: Amat = col1.row_join(col2) else: Amat = col1 else: Amat = col2 Amat_eq = msubs(Amat, op_point_dict) # Build up the B matrix if there are forcing variables # |0_(n + m)xs| # B = |B_u | if s != 0 and o - m + k != 0: Bmat = zeros(n + m, s).col_join(B_u) Bmat_eq = msubs(Bmat, op_point_dict) else: Bmat_eq = Matrix() # kwarg A_and_B indicates to return A, B for forming the equation # dx = [A]x + [B]r, where x = [q_indnd, u_indnd]^T, if A_and_B: A_cont = self.perm_mat.T * M_eq.LUsolve(Amat_eq) if Bmat_eq: B_cont = self.perm_mat.T * M_eq.LUsolve(Bmat_eq) else: # Bmat = Matrix([]), so no need to sub B_cont = Bmat_eq if simplify: A_cont.simplify() B_cont.simplify() return A_cont, B_cont # Otherwise return M, A, B for forming the equation # [M]dx = [A]x + [B]r, where x = [q, u]^T else: if simplify: M_eq.simplify() Amat_eq.simplify() Bmat_eq.simplify() return M_eq, Amat_eq, Bmat_eq
def linearize(self, op_point=None, A_and_B=False, simplify=False): """Linearize the system about the operating point. Note that q_op, u_op, qd_op, ud_op must satisfy the equations of motion. These may be either symbolic or numeric. Parameters ---------- op_point : dict or iterable of dicts, optional Dictionary or iterable of dictionaries containing the operating point conditions. These will be substituted in to the linearized system before the linearization is complete. Leave blank if you want a completely symbolic form. Note that any reduction in symbols (whether substituted for numbers or expressions with a common parameter) will result in faster runtime. A_and_B : bool, optional If A_and_B=False (default), (M, A, B) is returned for forming [M]*[q, u]^T = [A]*[q_ind, u_ind]^T + [B]r. If A_and_B=True, (A, B) is returned for forming dx = [A]x + [B]r, where x = [q_ind, u_ind]^T. simplify : bool, optional Determines if returned values are simplified before return. For large expressions this may be time consuming. Default is False. Potential Issues ---------------- Note that the process of solving with A_and_B=True is computationally intensive if there are many symbolic parameters. For this reason, it may be more desirable to use the default A_and_B=False, returning M, A, and B. More values may then be substituted in to these matrices later on. The state space form can then be found as A = P.T*M.LUsolve(A), B = P.T*M.LUsolve(B), where P = Linearizer.perm_mat. """ # Run the setup if needed: if not self._setup_done: self._setup() # Compose dict of operating conditions if isinstance(op_point, dict): op_point_dict = op_point elif isinstance(op_point, Iterable): op_point_dict = {} for op in op_point: op_point_dict.update(op) else: op_point_dict = {} # Extract dimension variables l, m, n, o, s, k = self._dims # Rename terms to shorten expressions M_qq = self._M_qq M_uqc = self._M_uqc M_uqd = self._M_uqd M_uuc = self._M_uuc M_uud = self._M_uud M_uld = self._M_uld A_qq = self._A_qq A_uqc = self._A_uqc A_uqd = self._A_uqd A_qu = self._A_qu A_uuc = self._A_uuc A_uud = self._A_uud B_u = self._B_u C_0 = self._C_0 C_1 = self._C_1 C_2 = self._C_2 # Build up Mass Matrix # |M_qq 0_nxo 0_nxk| # M = |M_uqc M_uuc 0_mxk| # |M_uqd M_uud M_uld| if o != 0: col2 = Matrix([zeros(n, o), M_uuc, M_uud]) if k != 0: col3 = Matrix([zeros(n + m, k), M_uld]) if n != 0: col1 = Matrix([M_qq, M_uqc, M_uqd]) if o != 0 and k != 0: M = col1.row_join(col2).row_join(col3) elif o != 0: M = col1.row_join(col2) else: M = col1 elif k != 0: M = col2.row_join(col3) else: M = col2 M_eq = msubs(M, op_point_dict) # Build up state coefficient matrix A # |(A_qq + A_qu*C_1)*C_0 A_qu*C_2| # A = |(A_uqc + A_uuc*C_1)*C_0 A_uuc*C_2| # |(A_uqd + A_uud*C_1)*C_0 A_uud*C_2| # Col 1 is only defined if n != 0 if n != 0: r1c1 = A_qq if o != 0: r1c1 += (A_qu * C_1) r1c1 = r1c1 * C_0 if m != 0: r2c1 = A_uqc if o != 0: r2c1 += (A_uuc * C_1) r2c1 = r2c1 * C_0 else: r2c1 = Matrix() if o - m + k != 0: r3c1 = A_uqd if o != 0: r3c1 += (A_uud * C_1) r3c1 = r3c1 * C_0 else: r3c1 = Matrix() col1 = Matrix([r1c1, r2c1, r3c1]) else: col1 = Matrix() # Col 2 is only defined if o != 0 if o != 0: if n != 0: r1c2 = A_qu * C_2 else: r1c2 = Matrix() if m != 0: r2c2 = A_uuc * C_2 else: r2c2 = Matrix() if o - m + k != 0: r3c2 = A_uud * C_2 else: r3c2 = Matrix() col2 = Matrix([r1c2, r2c2, r3c2]) else: col2 = Matrix() if col1: if col2: Amat = col1.row_join(col2) else: Amat = col1 else: Amat = col2 Amat_eq = msubs(Amat, op_point_dict) # Build up the B matrix if there are forcing variables # |0_(n + m)xs| # B = |B_u | if s != 0 and o - m + k != 0: Bmat = zeros(n + m, s).col_join(B_u) Bmat_eq = msubs(Bmat, op_point_dict) else: Bmat_eq = Matrix() # kwarg A_and_B indicates to return A, B for forming the equation # dx = [A]x + [B]r, where x = [q_indnd, u_indnd]^T, if A_and_B: A_cont = self.perm_mat.T * M_eq.LUsolve(Amat_eq) if Bmat_eq: B_cont = self.perm_mat.T * M_eq.LUsolve(Bmat_eq) else: # Bmat = Matrix([]), so no need to sub B_cont = Bmat_eq if simplify: A_cont.simplify() B_cont.simplify() return A_cont, B_cont # Otherwise return M, A, B for forming the equation # [M]dx = [A]x + [B]r, where x = [q, u]^T else: if simplify: M_eq.simplify() Amat_eq.simplify() Bmat_eq.simplify() return M_eq, Amat_eq, Bmat_eq
def orient(self, parent, rot_type, amounts, rot_order=''): """Defines the orientation of this frame relative to a parent frame. Parameters ========== parent : ReferenceFrame The frame that this ReferenceFrame will have its orientation matrix defined in relation to. rot_type : str The type of orientation matrix that is being created. Supported types are 'Body', 'Space', 'Quaternion', 'Axis', and 'DCM'. See examples for correct usage. amounts : list OR value The quantities that the orientation matrix will be defined by. In case of rot_type='DCM', value must be a sympy.matrices.MatrixBase object (or subclasses of it). rot_order : str or int If applicable, the order of a series of rotations. Examples ======== >>> from sympy.physics.vector import ReferenceFrame, Vector >>> from sympy import symbols, eye, ImmutableMatrix >>> q0, q1, q2, q3 = symbols('q0 q1 q2 q3') >>> N = ReferenceFrame('N') >>> B = ReferenceFrame('B') Now we have a choice of how to implement the orientation. First is Body. Body orientation takes this reference frame through three successive simple rotations. Acceptable rotation orders are of length 3, expressed in XYZ or 123, and cannot have a rotation about about an axis twice in a row. >>> B.orient(N, 'Body', [q1, q2, q3], 123) >>> B.orient(N, 'Body', [q1, q2, 0], 'ZXZ') >>> B.orient(N, 'Body', [0, 0, 0], 'XYX') Next is Space. Space is like Body, but the rotations are applied in the opposite order. >>> B.orient(N, 'Space', [q1, q2, q3], '312') Next is Quaternion. This orients the new ReferenceFrame with Quaternions, defined as a finite rotation about lambda, a unit vector, by some amount theta. This orientation is described by four parameters: q0 = cos(theta/2) q1 = lambda_x sin(theta/2) q2 = lambda_y sin(theta/2) q3 = lambda_z sin(theta/2) Quaternion does not take in a rotation order. >>> B.orient(N, 'Quaternion', [q0, q1, q2, q3]) Next is Axis. This is a rotation about an arbitrary, non-time-varying axis by some angle. The axis is supplied as a Vector. This is how simple rotations are defined. >>> B.orient(N, 'Axis', [q1, N.x + 2 * N.y]) Last is DCM (Direction Cosine Matrix). This is a rotation matrix given manually. >>> B.orient(N, 'DCM', eye(3)) >>> B.orient(N, 'DCM', ImmutableMatrix([[0, 1, 0], [0, 0, -1], [-1, 0, 0]])) """ from sympy.physics.vector.functions import dynamicsymbols _check_frame(parent) # Allow passing a rotation matrix manually. if rot_type == 'DCM': # When rot_type == 'DCM', then amounts must be a Matrix type object # (e.g. sympy.matrices.dense.MutableDenseMatrix). if not isinstance(amounts, MatrixBase): raise TypeError("Amounts must be a sympy Matrix type object.") else: amounts = list(amounts) for i, v in enumerate(amounts): if not isinstance(v, Vector): amounts[i] = sympify(v) def _rot(axis, angle): """DCM for simple axis 1,2,or 3 rotations. """ if axis == 1: return Matrix([[1, 0, 0], [0, cos(angle), -sin(angle)], [0, sin(angle), cos(angle)]]) elif axis == 2: return Matrix([[cos(angle), 0, sin(angle)], [0, 1, 0], [-sin(angle), 0, cos(angle)]]) elif axis == 3: return Matrix([[cos(angle), -sin(angle), 0], [sin(angle), cos(angle), 0], [0, 0, 1]]) approved_orders = ('123', '231', '312', '132', '213', '321', '121', '131', '212', '232', '313', '323', '') # make sure XYZ => 123 and rot_type is in upper case rot_order = translate(str(rot_order), 'XYZxyz', '123123') rot_type = rot_type.upper() if not rot_order in approved_orders: raise TypeError('The supplied order is not an approved type') parent_orient = [] if rot_type == 'AXIS': if not rot_order == '': raise TypeError('Axis orientation takes no rotation order') if not (isinstance(amounts, (list, tuple)) & (len(amounts) == 2)): raise TypeError('Amounts are a list or tuple of length 2') theta = amounts[0] axis = amounts[1] axis = _check_vector(axis) if not axis.dt(parent) == 0: raise ValueError('Axis cannot be time-varying') axis = axis.express(parent).normalize() axis = axis.args[0][0] parent_orient = ( (eye(3) - axis * axis.T) * cos(theta) + Matrix([[0, -axis[2], axis[1]], [axis[2], 0, -axis[0]], [-axis[1], axis[0], 0]]) * sin(theta) + axis * axis.T) elif rot_type == 'QUATERNION': if not rot_order == '': raise TypeError( 'Quaternion orientation takes no rotation order') if not (isinstance(amounts, (list, tuple)) & (len(amounts) == 4)): raise TypeError('Amounts are a list or tuple of length 4') q0, q1, q2, q3 = amounts parent_orient = (Matrix([[ q0**2 + q1**2 - q2**2 - q3**2, 2 * (q1 * q2 - q0 * q3), 2 * (q0 * q2 + q1 * q3) ], [ 2 * (q1 * q2 + q0 * q3), q0**2 - q1**2 + q2**2 - q3**2, 2 * (q2 * q3 - q0 * q1) ], [ 2 * (q1 * q3 - q0 * q2), 2 * (q0 * q1 + q2 * q3), q0**2 - q1**2 - q2**2 + q3**2 ]])) elif rot_type == 'BODY': if not (len(amounts) == 3 & len(rot_order) == 3): raise TypeError('Body orientation takes 3 values & 3 orders') a1 = int(rot_order[0]) a2 = int(rot_order[1]) a3 = int(rot_order[2]) parent_orient = (_rot(a1, amounts[0]) * _rot(a2, amounts[1]) * _rot(a3, amounts[2])) elif rot_type == 'SPACE': if not (len(amounts) == 3 & len(rot_order) == 3): raise TypeError('Space orientation takes 3 values & 3 orders') a1 = int(rot_order[0]) a2 = int(rot_order[1]) a3 = int(rot_order[2]) parent_orient = (_rot(a3, amounts[2]) * _rot(a2, amounts[1]) * _rot(a1, amounts[0])) elif rot_type == 'DCM': parent_orient = amounts else: raise NotImplementedError('That is not an implemented rotation') #Reset the _dcm_cache of this frame, and remove it from the _dcm_caches #of the frames it is linked to. Also remove it from the _dcm_dict of #its parent frames = self._dcm_cache.keys() dcm_dict_del = [] dcm_cache_del = [] for frame in frames: if frame in self._dcm_dict: dcm_dict_del += [frame] dcm_cache_del += [frame] for frame in dcm_dict_del: del frame._dcm_dict[self] for frame in dcm_cache_del: del frame._dcm_cache[self] #Add the dcm relationship to _dcm_dict self._dcm_dict = self._dlist[0] = {} self._dcm_dict.update({parent: parent_orient.T}) parent._dcm_dict.update({self: parent_orient}) #Also update the dcm cache after resetting it self._dcm_cache = {} self._dcm_cache.update({parent: parent_orient.T}) parent._dcm_cache.update({self: parent_orient}) if rot_type == 'QUATERNION': t = dynamicsymbols._t q0, q1, q2, q3 = amounts q0d = diff(q0, t) q1d = diff(q1, t) q2d = diff(q2, t) q3d = diff(q3, t) w1 = 2 * (q1d * q0 + q2d * q3 - q3d * q2 - q0d * q1) w2 = 2 * (q2d * q0 + q3d * q1 - q1d * q3 - q0d * q2) w3 = 2 * (q3d * q0 + q1d * q2 - q2d * q1 - q0d * q3) wvec = Vector([(Matrix([w1, w2, w3]), self)]) elif rot_type == 'AXIS': thetad = (amounts[0]).diff(dynamicsymbols._t) wvec = thetad * amounts[1].express(parent).normalize() elif rot_type == 'DCM': wvec = self._w_diff_dcm(parent) else: try: from sympy.polys.polyerrors import CoercionFailed from sympy.physics.vector.functions import kinematic_equations q1, q2, q3 = amounts u1, u2, u3 = symbols('u1, u2, u3', cls=Dummy) templist = kinematic_equations([u1, u2, u3], [q1, q2, q3], rot_type, rot_order) templist = [expand(i) for i in templist] td = solve(templist, [u1, u2, u3]) u1 = expand(td[u1]) u2 = expand(td[u2]) u3 = expand(td[u3]) wvec = u1 * self.x + u2 * self.y + u3 * self.z except (CoercionFailed, AssertionError): wvec = self._w_diff_dcm(parent) self._ang_vel_dict.update({parent: wvec}) parent._ang_vel_dict.update({self: -wvec}) self._var_dict = {}
def __init__(self, f_0, f_1, f_2, f_3, f_4, f_c, f_v, f_a, q, u, q_i=None, q_d=None, u_i=None, u_d=None, r=None, lams=None): """ Parameters ---------- f_0, f_1, f_2, f_3, f_4, f_c, f_v, f_a : array_like System of equations holding the general system form. Supply empty array or Matrix if the parameter doesn't exist. q : array_like The generalized coordinates. u : array_like The generalized speeds q_i, u_i : array_like, optional The independent generalized coordinates and speeds. q_d, u_d : array_like, optional The dependent generalized coordinates and speeds. r : array_like, optional The input variables. lams : array_like, optional The lagrange multipliers """ # Generalized equation form self.f_0 = Matrix(f_0) self.f_1 = Matrix(f_1) self.f_2 = Matrix(f_2) self.f_3 = Matrix(f_3) self.f_4 = Matrix(f_4) self.f_c = Matrix(f_c) self.f_v = Matrix(f_v) self.f_a = Matrix(f_a) # Generalized equation variables self.q = Matrix(q) self.u = Matrix(u) none_handler = lambda x: Matrix(x) if x else Matrix() self.q_i = none_handler(q_i) self.q_d = none_handler(q_d) self.u_i = none_handler(u_i) self.u_d = none_handler(u_d) self.r = none_handler(r) self.lams = none_handler(lams) # Derivatives of generalized equation variables self._qd = self.q.diff(dynamicsymbols._t) self._ud = self.u.diff(dynamicsymbols._t) # If the user doesn't actually use generalized variables, and the # qd and u vectors have any intersecting variables, this can cause # problems. We'll fix this with some hackery, and Dummy variables dup_vars = set(self._qd).intersection(self.u) self._qd_dup = Matrix([var if var not in dup_vars else Dummy() for var in self._qd]) # Derive dimesion terms l = len(self.f_c) m = len(self.f_v) n = len(self.q) o = len(self.u) s = len(self.r) k = len(self.lams) dims = namedtuple('dims', ['l', 'm', 'n', 'o', 's', 'k']) self._dims = dims(l, m, n, o, s, k) self._setup_done = False
def __init__(self, Lagrangian, qs, coneqs=None, forcelist=None, bodies=None, frame=None, hol_coneqs=None, nonhol_coneqs=None): """Supply the following for the initialization of LagrangesMethod Lagrangian : Sympifyable qs : array_like The generalized coordinates hol_coneqs : array_like, optional The holonomic constraint equations nonhol_coneqs : array_like, optional The nonholonomic constraint equations forcelist : iterable, optional Takes an iterable of (Point, Vector) or (ReferenceFrame, Vector) tuples which represent the force at a point or torque on a frame. This feature is primarily to account for the nonconservative forces and/or moments. bodies : iterable, optional Takes an iterable containing the rigid bodies and particles of the system. frame : ReferenceFrame, optional Supply the inertial frame. This is used to determine the generalized forces due to non-conservative forces. """ self._L = Matrix([sympify(Lagrangian)]) self.eom = None self._m_cd = Matrix() # Mass Matrix of differentiated coneqs self._m_d = Matrix() # Mass Matrix of dynamic equations self._f_cd = Matrix() # Forcing part of the diff coneqs self._f_d = Matrix() # Forcing part of the dynamic equations self.lam_coeffs = Matrix() # The coeffecients of the multipliers forcelist = forcelist if forcelist else [] if not iterable(forcelist): raise TypeError('Force pairs must be supplied in an iterable.') self._forcelist = forcelist if frame and not isinstance(frame, ReferenceFrame): raise TypeError('frame must be a valid ReferenceFrame') self._bodies = bodies self.inertial = frame self.lam_vec = Matrix() self._term1 = Matrix() self._term2 = Matrix() self._term3 = Matrix() self._term4 = Matrix() # Creating the qs, qdots and qdoubledots if not iterable(qs): raise TypeError('Generalized coordinates must be an iterable') self._q = Matrix(qs) self._qdots = self.q.diff(dynamicsymbols._t) self._qdoubledots = self._qdots.diff(dynamicsymbols._t) # Deal with constraint equations if coneqs: SymPyDeprecationWarning("The `coneqs` kwarg is deprecated in " "favor of `hol_coneqs` and `nonhol_coneqs`. Please " "update your code").warn() self.coneqs = coneqs else: mat_build = lambda x: Matrix(x) if x else Matrix() hol_coneqs = mat_build(hol_coneqs) nonhol_coneqs = mat_build(nonhol_coneqs) self.coneqs = Matrix([hol_coneqs.diff(dynamicsymbols._t), nonhol_coneqs]) self._hol_coneqs = hol_coneqs
class KanesMethod(object): """Kane's method object. This object is used to do the "book-keeping" as you go through and form equations of motion in the way Kane presents in: Kane, T., Levinson, D. Dynamics Theory and Applications. 1985 McGraw-Hill The attributes are for equations in the form [M] udot = forcing. Attributes ========== q, u : Matrix Matrices of the generalized coordinates and speeds bodylist : iterable Iterable of Point and RigidBody objects in the system. forcelist : iterable Iterable of (Point, vector) or (ReferenceFrame, vector) tuples describing the forces on the system. auxiliary : Matrix If applicable, the set of auxiliary Kane's equations used to solve for non-contributing forces. mass_matrix : Matrix The system's mass matrix forcing : Matrix The system's forcing vector mass_matrix_full : Matrix The "mass matrix" for the u's and q's forcing_full : Matrix The "forcing vector" for the u's and q's Examples ======== This is a simple example for a one degree of freedom translational spring-mass-damper. In this example, we first need to do the kinematics. This involves creating generalized speeds and coordinates and their derivatives. Then we create a point and set its velocity in a frame. >>> from sympy import symbols >>> from sympy.physics.mechanics import dynamicsymbols, ReferenceFrame >>> from sympy.physics.mechanics import Point, Particle, KanesMethod >>> q, u = dynamicsymbols('q u') >>> qd, ud = dynamicsymbols('q u', 1) >>> m, c, k = symbols('m c k') >>> N = ReferenceFrame('N') >>> P = Point('P') >>> P.set_vel(N, u * N.x) Next we need to arrange/store information in the way that KanesMethod requires. The kinematic differential equations need to be stored in a dict. A list of forces/torques must be constructed, where each entry in the list is a (Point, Vector) or (ReferenceFrame, Vector) tuple, where the Vectors represent the Force or Torque. Next a particle needs to be created, and it needs to have a point and mass assigned to it. Finally, a list of all bodies and particles needs to be created. >>> kd = [qd - u] >>> FL = [(P, (-k * q - c * u) * N.x)] >>> pa = Particle('pa', P, m) >>> BL = [pa] Finally we can generate the equations of motion. First we create the KanesMethod object and supply an inertial frame, coordinates, generalized speeds, and the kinematic differential equations. Additional quantities such as configuration and motion constraints, dependent coordinates and speeds, and auxiliary speeds are also supplied here (see the online documentation). Next we form FR* and FR to complete: Fr + Fr* = 0. We have the equations of motion at this point. It makes sense to rearrnge them though, so we calculate the mass matrix and the forcing terms, for E.o.M. in the form: [MM] udot = forcing, where MM is the mass matrix, udot is a vector of the time derivatives of the generalized speeds, and forcing is a vector representing "forcing" terms. >>> KM = KanesMethod(N, q_ind=[q], u_ind=[u], kd_eqs=kd) >>> (fr, frstar) = KM.kanes_equations(BL, FL) >>> MM = KM.mass_matrix >>> forcing = KM.forcing >>> rhs = MM.inv() * forcing >>> rhs Matrix([[(-c*u(t) - k*q(t))/m]]) >>> KM.linearize(A_and_B=True, new_method=True)[0] Matrix([ [ 0, 1], [-k/m, -c/m]]) Please look at the documentation pages for more information on how to perform linearization and how to deal with dependent coordinates & speeds, and how do deal with bringing non-contributing forces into evidence. """ def __init__(self, frame, q_ind, u_ind, kd_eqs=None, q_dependent=None, configuration_constraints=None, u_dependent=None, velocity_constraints=None, acceleration_constraints=None, u_auxiliary=None): """Please read the online documentation. """ if not isinstance(frame, ReferenceFrame): raise TypeError('An intertial ReferenceFrame must be supplied') self._inertial = frame self._fr = None self._frstar = None self._forcelist = None self._bodylist = None self._initialize_vectors(q_ind, q_dependent, u_ind, u_dependent, u_auxiliary) self._initialize_kindiffeq_matrices(kd_eqs) self._initialize_constraint_matrices(configuration_constraints, velocity_constraints, acceleration_constraints) def _initialize_vectors(self, q_ind, q_dep, u_ind, u_dep, u_aux): """Initialize the coordinate and speed vectors.""" none_handler = lambda x: Matrix(x) if x else Matrix() # Initialize generalized coordinates q_dep = none_handler(q_dep) if not iterable(q_ind): raise TypeError('Generalized coordinates must be an iterable.') if not iterable(q_dep): raise TypeError('Dependent coordinates must be an iterable.') q_ind = Matrix(q_ind) self._qdep = q_dep self._q = Matrix([q_ind, q_dep]) self._qdot = self.q.diff(dynamicsymbols._t) # Initialize generalized speeds u_dep = none_handler(u_dep) if not iterable(u_ind): raise TypeError('Generalized speeds must be an iterable.') if not iterable(u_dep): raise TypeError('Dependent speeds must be an iterable.') u_ind = Matrix(u_ind) self._udep = u_dep self._u = Matrix([u_ind, u_dep]) self._udot = self.u.diff(dynamicsymbols._t) self._uaux = none_handler(u_aux) def _initialize_constraint_matrices(self, config, vel, acc): """Initializes constraint matrices.""" # Define vector dimensions o = len(self.u) m = len(self._udep) p = o - m none_handler = lambda x: Matrix(x) if x else Matrix() # Initialize configuration constraints config = none_handler(config) if len(self._qdep) != len(config): raise ValueError('There must be an equal number of dependent ' 'coordinates and configuration constraints.') self._f_h = none_handler(config) # Initialize velocity and acceleration constraints vel = none_handler(vel) acc = none_handler(acc) if len(vel) != m: raise ValueError('There must be an equal number of dependent ' 'speeds and velocity constraints.') if acc and (len(acc) != m): raise ValueError('There must be an equal number of dependent ' 'speeds and acceleration constraints.') if vel: u_zero = dict((i, 0) for i in self.u) udot_zero = dict((i, 0) for i in self._udot) # When calling kanes_equations, another class instance will be # created if auxiliary u's are present. In this case, the # computation of kinetic differential equation matrices will be # skipped as this was computed during the original KanesMethod # object, and the qd_u_map will not be available. if self._qdot_u_map is not None: vel = msubs(vel, self._qdot_u_map) self._f_nh = msubs(vel, u_zero) self._k_nh = (vel - self._f_nh).jacobian(self.u) # If no acceleration constraints given, calculate them. if not acc: self._f_dnh = (self._k_nh.diff(dynamicsymbols._t) * self.u + self._f_nh.diff(dynamicsymbols._t)) self._k_dnh = self._k_nh else: if self._qdot_u_map is not None: acc = msubs(acc, self._qdot_u_map) self._f_dnh = msubs(acc, udot_zero) self._k_dnh = (acc - self._f_dnh).jacobian(self._udot) # Form of non-holonomic constraints is B*u + C = 0. # We partition B into independent and dependent columns: # Ars is then -B_dep.inv() * B_ind, and it relates dependent speeds # to independent speeds as: udep = Ars*uind, neglecting the C term. B_ind = self._k_nh[:, :p] B_dep = self._k_nh[:, p:o] self._Ars = -B_dep.LUsolve(B_ind) else: self._f_nh = Matrix() self._k_nh = Matrix() self._f_dnh = Matrix() self._k_dnh = Matrix() self._Ars = Matrix() def _initialize_kindiffeq_matrices(self, kdeqs): """Initialize the kinematic differential equation matrices.""" if kdeqs: if len(self.q) != len(kdeqs): raise ValueError('There must be an equal number of kinematic ' 'differential equations and coordinates.') kdeqs = Matrix(kdeqs) u = self.u qdot = self._qdot # Dictionaries setting things to zero u_zero = dict((i, 0) for i in u) uaux_zero = dict((i, 0) for i in self._uaux) qdot_zero = dict((i, 0) for i in qdot) f_k = msubs(kdeqs, u_zero, qdot_zero) k_ku = (msubs(kdeqs, qdot_zero) - f_k).jacobian(u) k_kqdot = (msubs(kdeqs, u_zero) - f_k).jacobian(qdot) f_k = k_kqdot.LUsolve(f_k) k_ku = k_kqdot.LUsolve(k_ku) k_kqdot = eye(len(qdot)) self._qdot_u_map = solve_linear_system_LU( Matrix([k_kqdot.T, -(k_ku * u + f_k).T]).T, qdot) self._f_k = msubs(f_k, uaux_zero) self._k_ku = msubs(k_ku, 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() 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_frstar(self, bl): """Form the generalized inertia force.""" if not iterable(bl): raise TypeError('Bodies must be supplied in an iterable.') t = dynamicsymbols._t N = self._inertial # Dicts setting things to zero udot_zero = dict((i, 0) for i in self._udot) uaux_zero = dict((i, 0) for i in self._uaux) uauxdot = [diff(i, t) for i in self._uaux] uauxdot_zero = dict((i, 0) for i in uauxdot) # Dictionary of q' and q'' to u and u' q_ddot_u_map = dict( (k.diff(t), v.diff(t)) for (k, v) in self._qdot_u_map.items()) q_ddot_u_map.update(self._qdot_u_map) # Fill up the list of partials: format is a list with num 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. def get_partial_velocity(body): if isinstance(body, RigidBody): vlist = [body.masscenter.vel(N), body.frame.ang_vel_in(N)] elif isinstance(body, Particle): vlist = [ body.point.vel(N), ] else: raise TypeError('The body list may only contain either ' 'RigidBody or Particle as list elements.') v = [msubs(vel, self._qdot_u_map) for vel in vlist] return partial_velocity(v, self.u, N) partials = [get_partial_velocity(body) for body in bl] # Compute fr_star in two components: # fr_star = -(MM*u' + nonMM) o = len(self.u) MM = zeros(o, o) nonMM = zeros(o, 1) zero_uaux = lambda expr: msubs(expr, uaux_zero) zero_udot_uaux = lambda expr: msubs(msubs(expr, udot_zero), uaux_zero) for i, body in enumerate(bl): if isinstance(body, RigidBody): M = zero_uaux(body.mass) I = zero_uaux(body.central_inertia) vel = zero_uaux(body.masscenter.vel(N)) omega = zero_uaux(body.frame.ang_vel_in(N)) acc = zero_udot_uaux(body.masscenter.acc(N)) inertial_force = (M.diff(t) * vel + M * acc) inertial_torque = zero_uaux( (I.dt(body.frame) & omega) + msubs(I & body.frame.ang_acc_in(N), udot_zero) + (omega ^ (I & omega))) for j in range(o): tmp_vel = zero_uaux(partials[i][0][j]) tmp_ang = zero_uaux(I & partials[i][1][j]) 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] else: M = zero_uaux(body.mass) vel = zero_uaux(body.point.vel(N)) acc = zero_udot_uaux(body.point.acc(N)) inertial_force = (M.diff(t) * vel + M * acc) for j in range(o): temp = zero_uaux(partials[i][0][j]) for k in range(o): MM[j, k] += M * (temp & partials[i][0][k]) nonMM[j] += inertial_force & partials[i][0][j] # Compose fr_star out of MM and nonMM MM = zero_uaux(msubs(MM, q_ddot_u_map)) nonMM = msubs(msubs(nonMM, q_ddot_u_map), udot_zero, uauxdot_zero, uaux_zero) fr_star = -(MM * msubs(Matrix(self._udot), uauxdot_zero) + nonMM) # If there are dependent speeds, we need to find fr_star_tilde if self._udep: p = o - len(self._udep) fr_star_ind = fr_star[:p, 0] fr_star_dep = fr_star[p:o, 0] fr_star = fr_star_ind + (self._Ars.T * fr_star_dep) # Apply the same to MM MMi = MM[:p, :] MMd = MM[p:o, :] MM = MMi + (self._Ars.T * MMd) self._bodylist = bl self._frstar = fr_star self._k_d = MM self._f_d = -msubs(self._fr + self._frstar, udot_zero) return fr_star 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 linearize(self, **kwargs): """ Linearize the equations of motion about a symbolic operating point. If kwarg A_and_B is False (default), returns M, A, B, r for the linearized form, M*[q', u']^T = A*[q_ind, u_ind]^T + B*r. If kwarg A_and_B is True, returns A, B, r for the linearized form dx = A*x + B*r, where x = [q_ind, u_ind]^T. Note that this is computationally intensive if there are many symbolic parameters. For this reason, it may be more desirable to use the default A_and_B=False, returning M, A, and B. Values may then be substituted in to these matrices, and the state space form found as A = P.T*M.inv()*A, B = P.T*M.inv()*B, where P = Linearizer.perm_mat. In both cases, r is found as all dynamicsymbols in the equations of motion that are not part of q, u, q', or u'. They are sorted in canonical form. The operating points may be also entered using the ``op_point`` kwarg. This takes a dictionary of {symbol: value}, or a an iterable of such dictionaries. The values may be numberic or symbolic. The more values you can specify beforehand, the faster this computation will run. As part of the deprecation cycle, the new method will not be used unless the kwarg ``new_method`` is set to True. If the kwarg is missing, or set to false, the old linearization method will be used. After next release the need for this kwarg will be removed. For more documentation, please see the ``Linearizer`` class.""" if 'new_method' not in kwargs or not kwargs['new_method']: # User is still using old code. SymPyDeprecationWarning( 'The linearize class method has changed ' 'to a new interface, the old method is deprecated. To ' 'use the new method, set the kwarg `new_method=True`. ' 'For more information, read the docstring ' 'of `linearize`.').warn() return self._old_linearize() # Remove the new method flag, before passing kwargs to linearize kwargs.pop('new_method') linearizer = self.to_linearizer() result = linearizer.linearize(**kwargs) return result + (linearizer.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( zip(uaux[:] + uauxdot[:], [0] * (len(uaux) + len(uauxdot)))) # Checking for dynamic symbols outside the dynamic differential # equations; throws error if there is. insyms = set(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(msubs(self._f_d, 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 = msubs(f1, subdict) f2 = msubs(f2, subdict) fh = msubs(self._f_h, subdict) fku = msubs(self._k_ku * Matrix(self.u), subdict) fkf = msubs(self._f_k, 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 kanes_equations(self, bodies, loads=None): """ Method to form Kane's equations, Fr + Fr* = 0. Returns (Fr, Fr*). In the case where auxiliary generalized speeds are present (say, s auxiliary speeds, o generalized speeds, and m motion constraints) the length of the returned vectors will be o - m + s in length. The first o - m equations will be the constrained Kane's equations, then the s auxiliary Kane's equations. These auxiliary equations can be accessed with the auxiliary_eqs(). Parameters ========== bodies : iterable An iterable of all RigidBody's and Particle's in the system. A system must have at least one body. loads : iterable Takes in an iterable of (Particle, Vector) or (ReferenceFrame, Vector) tuples which represent the force at a point or torque on a frame. Must be either a non-empty iterable of tuples or None which corresponds to a system with no constraints. """ if (bodies is None and loads != None) or isinstance(bodies[0], tuple): # This switches the order if they use the old way. bodies, loads = loads, bodies SymPyDeprecationWarning( value='The API for kanes_equations() has changed such ' 'that the loads (forces and torques) are now the second argument ' 'and is optional with None being the default.', feature='The kanes_equation() argument order', useinstead= 'switched argument order to update your code, For example: ' 'kanes_equations(loads, bodies) > kanes_equations(bodies, loads).', issue=10945, deprecated_since_version="1.1").warn() if not self._k_kqdot: raise AttributeError( 'Create an instance of KanesMethod with ' 'kinematic differential equations to use this method.') fr = self._form_fr(loads) frstar = self._form_frstar(bodies) if self._uaux: if not self._udep: km = KanesMethod(self._inertial, self.q, self._uaux, u_auxiliary=self._uaux) else: km = KanesMethod(self._inertial, self.q, self._uaux, u_auxiliary=self._uaux, u_dependent=self._udep, velocity_constraints=(self._k_nh * self.u + self._f_nh)) km._qdot_u_map = self._qdot_u_map self._km = km fraux = km._form_fr(loads) frstaraux = km._form_frstar(bodies) self._aux_eq = fraux + frstaraux self._fr = fr.col_join(fraux) self._frstar = frstar.col_join(frstaraux) return (self._fr, self._frstar) def rhs(self, inv_method=None): """Returns the system's equations of motion in first order form. The output is the right hand side of:: x' = |q'| =: f(q, u, r, p, t) |u'| The right hand side is what is needed by most numerical ODE integrators. Parameters ========== inv_method : str The specific sympy inverse matrix calculation method to use. For a list of valid methods, see :meth:`~sympy.matrices.matrices.MatrixBase.inv` """ rhs = zeros(len(self.q) + len(self.u), c=1) kdes = self.kindiffdict() for i, q_i in enumerate(self.q): rhs[i] = kdes[q_i.diff()] if inv_method is None: rhs[len(self.q):, 0] = self.mass_matrix.LUsolve(self.forcing) else: rhs[len(self.q):, 0] = (self.mass_matrix.inv(inv_method, try_block_diag=True) * self.forcing) return rhs def kindiffdict(self): """Returns a dictionary mapping q' to u.""" if not self._qdot_u_map: raise AttributeError( 'Create an instance of KanesMethod with ' 'kinematic differential equations to use this method.') return self._qdot_u_map @property def auxiliary_eqs(self): """A matrix containing the auxiliary equations.""" if not self._fr or not self._frstar: raise ValueError('Need to compute Fr, Fr* first.') if not self._uaux: raise ValueError('No auxiliary speeds have been declared.') return self._aux_eq @property def mass_matrix(self): """The mass matrix of the system.""" if not self._fr or not self._frstar: raise ValueError('Need to compute Fr, Fr* first.') return Matrix([self._k_d, self._k_dnh]) @property def mass_matrix_full(self): """The mass matrix of the system, augmented by the kinematic differential equations.""" if not self._fr or not self._frstar: raise ValueError('Need to compute Fr, Fr* first.') o = len(self.u) n = len(self.q) return ((self._k_kqdot).row_join(zeros(n, o))).col_join( (zeros(o, n)).row_join(self.mass_matrix)) @property def forcing(self): """The forcing vector of the system.""" if not self._fr or not self._frstar: raise ValueError('Need to compute Fr, Fr* first.') return -Matrix([self._f_d, self._f_dnh]) @property def forcing_full(self): """The forcing vector of the system, augmented by the kinematic differential equations.""" if not self._fr or not self._frstar: raise ValueError('Need to compute Fr, Fr* first.') f1 = self._k_ku * Matrix(self.u) + self._f_k return -Matrix([f1, self._f_d, self._f_dnh]) @property def q(self): return self._q @property def u(self): return self._u @property def bodylist(self): return self._bodylist @property def forcelist(self): return self._forcelist
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 _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( zip(uaux[:] + uauxdot[:], [0] * (len(uaux) + len(uauxdot)))) # Checking for dynamic symbols outside the dynamic differential # equations; throws error if there is. insyms = set(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(msubs(self._f_d, 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 = msubs(f1, subdict) f2 = msubs(f2, subdict) fh = msubs(self._f_h, subdict) fku = msubs(self._k_ku * Matrix(self.u), subdict) fkf = msubs(self._f_k, 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_constraint_matrices(self, config, vel, acc): """Initializes constraint matrices.""" # Define vector dimensions o = len(self.u) m = len(self._udep) p = o - m none_handler = lambda x: Matrix(x) if x else Matrix() # Initialize configuration constraints config = none_handler(config) if len(self._qdep) != len(config): raise ValueError('There must be an equal number of dependent ' 'coordinates and configuration constraints.') self._f_h = none_handler(config) # Initialize velocity and acceleration constraints vel = none_handler(vel) acc = none_handler(acc) if len(vel) != m: raise ValueError('There must be an equal number of dependent ' 'speeds and velocity constraints.') if acc and (len(acc) != m): raise ValueError('There must be an equal number of dependent ' 'speeds and acceleration constraints.') if vel: u_zero = dict((i, 0) for i in self.u) udot_zero = dict((i, 0) for i in self._udot) # When calling kanes_equations, another class instance will be # created if auxiliary u's are present. In this case, the # computation of kinetic differential equation matrices will be # skipped as this was computed during the original KanesMethod # object, and the qd_u_map will not be available. if self._qdot_u_map is not None: vel = msubs(vel, self._qdot_u_map) self._f_nh = msubs(vel, u_zero) self._k_nh = (vel - self._f_nh).jacobian(self.u) # If no acceleration constraints given, calculate them. if not acc: self._f_dnh = (self._k_nh.diff(dynamicsymbols._t) * self.u + self._f_nh.diff(dynamicsymbols._t)) self._k_dnh = self._k_nh else: if self._qdot_u_map is not None: acc = msubs(acc, self._qdot_u_map) self._f_dnh = msubs(acc, udot_zero) self._k_dnh = (acc - self._f_dnh).jacobian(self._udot) # Form of non-holonomic constraints is B*u + C = 0. # We partition B into independent and dependent columns: # Ars is then -B_dep.inv() * B_ind, and it relates dependent speeds # to independent speeds as: udep = Ars*uind, neglecting the C term. B_ind = self._k_nh[:, :p] B_dep = self._k_nh[:, p:o] self._Ars = -B_dep.LUsolve(B_ind) else: self._f_nh = Matrix() self._k_nh = Matrix() self._f_dnh = Matrix() self._k_dnh = Matrix() self._Ars = Matrix()
def matrix_form(self, weylelt): """ This method takes input from the user in the form of products of the generating reflections, and returns the matrix corresponding to the element of the Weyl group. Since each element of the Weyl group is a reflection of some type, there is a corresponding matrix representation. This method uses the standard representation for all the generating reflections. Examples ======== >>> from sympy.liealgebras.weyl_group import WeylGroup >>> f = WeylGroup("F4") >>> f.matrix_form('r2*r3') Matrix([ [1, 0, 0, 0], [0, 1, 0, 0], [0, 0, 0, -1], [0, 0, 1, 0]]) """ elts = list(weylelt) reflections = elts[1::3] n = self.cartan_type.rank() if self.cartan_type.series == "A": matrixform = eye(n + 1) for elt in reflections: a = int(elt) mat = eye(n + 1) mat[a - 1, a - 1] = 0 mat[a - 1, a] = 1 mat[a, a - 1] = 1 mat[a, a] = 0 matrixform *= mat return matrixform if self.cartan_type.series == "D": matrixform = eye(n) for elt in reflections: a = int(elt) mat = eye(n) if a < n: mat[a - 1, a - 1] = 0 mat[a - 1, a] = 1 mat[a, a - 1] = 1 mat[a, a] = 0 matrixform *= mat else: mat[n - 2, n - 1] = -1 mat[n - 2, n - 2] = 0 mat[n - 1, n - 2] = -1 mat[n - 1, n - 1] = 0 matrixform *= mat return matrixform if self.cartan_type.series == "G": matrixform = eye(3) for elt in reflections: a = int(elt) if a == 1: gen1 = Matrix([[1, 0, 0], [0, 0, 1], [0, 1, 0]]) matrixform *= gen1 else: gen2 = Matrix( [ [Rational(2, 3), Rational(2, 3), Rational(-1, 3)], [Rational(2, 3), Rational(-1, 3), Rational(2, 3)], [Rational(-1, 3), Rational(2, 3), Rational(2, 3)], ] ) matrixform *= gen2 return matrixform if self.cartan_type.series == "F": matrixform = eye(4) for elt in reflections: a = int(elt) if a == 1: mat = Matrix( [[1, 0, 0, 0], [0, 0, 1, 0], [0, 1, 0, 0], [0, 0, 0, 1]] ) matrixform *= mat elif a == 2: mat = Matrix( [[1, 0, 0, 0], [0, 1, 0, 0], [0, 0, 0, 1], [0, 0, 1, 0]] ) matrixform *= mat elif a == 3: mat = Matrix( [[1, 0, 0, 0], [0, 1, 0, 0], [0, 0, 1, 0], [0, 0, 0, -1]] ) matrixform *= mat else: mat = Matrix( [ [ Rational(1, 2), Rational(1, 2), Rational(1, 2), Rational(1, 2), ], [ Rational(1, 2), Rational(1, 2), Rational(-1, 2), Rational(-1, 2), ], [ Rational(1, 2), Rational(-1, 2), Rational(1, 2), Rational(-1, 2), ], [ Rational(1, 2), Rational(-1, 2), Rational(-1, 2), Rational(1, 2), ], ] ) matrixform *= mat return matrixform if self.cartan_type.series == "E": matrixform = eye(8) for elt in reflections: a = int(elt) if a == 1: mat = Matrix( [ [ Rational(3, 4), Rational(1, 4), Rational(1, 4), Rational(1, 4), Rational(1, 4), Rational(1, 4), Rational(1, 4), Rational(-1, 4), ], [ Rational(1, 4), Rational(3, 4), Rational(-1, 4), Rational(-1, 4), Rational(-1, 4), Rational(-1, 4), Rational(1, 4), Rational(-1, 4), ], [ Rational(1, 4), Rational(-1, 4), Rational(3, 4), Rational(-1, 4), Rational(-1, 4), Rational(-1, 4), Rational(-1, 4), Rational(1, 4), ], [ Rational(1, 4), Rational(-1, 4), Rational(-1, 4), Rational(3, 4), Rational(-1, 4), Rational(-1, 4), Rational(-1, 4), Rational(1, 4), ], [ Rational(1, 4), Rational(-1, 4), Rational(-1, 4), Rational(-1, 4), Rational(3, 4), Rational(-1, 4), Rational(-1, 4), Rational(1, 4), ], [ Rational(1, 4), Rational(-1, 4), Rational(-1, 4), Rational(-1, 4), Rational(-1, 4), Rational(3, 4), Rational(-1, 4), Rational(1, 4), ], [ Rational(1, 4), Rational(-1, 4), Rational(-1, 4), Rational(-1, 4), Rational(-1, 4), Rational(-1, 4), Rational(-3, 4), Rational(1, 4), ], [ Rational(1, 4), Rational(-1, 4), Rational(-1, 4), Rational(-1, 4), Rational(-1, 4), Rational(-1, 4), Rational(-1, 4), Rational(3, 4), ], ] ) matrixform *= mat elif a == 2: mat = eye(8) mat[0, 0] = 0 mat[0, 1] = -1 mat[1, 0] = -1 mat[1, 1] = 0 matrixform *= mat else: mat = eye(8) mat[a - 3, a - 3] = 0 mat[a - 3, a - 2] = 1 mat[a - 2, a - 3] = 1 mat[a - 2, a - 2] = 0 matrixform *= mat return matrixform if self.cartan_type.series == "B" or self.cartan_type.series == "C": matrixform = eye(n) for elt in reflections: a = int(elt) mat = eye(n) if a == 1: mat[0, 0] = -1 matrixform *= mat else: mat[a - 2, a - 2] = 0 mat[a - 2, a - 1] = 1 mat[a - 1, a - 2] = 1 mat[a - 1, a - 1] = 0 matrixform *= mat return matrixform
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 forcing(self): """The forcing vector of the system.""" if not self._fr or not self._frstar: raise ValueError('Need to compute Fr, Fr* first.') return -Matrix([self._f_d, self._f_dnh])
class SymbolicSystem(object): """SymbolicSystem is a class that contains all the information about a system in a symbolic format such as the equations of motions and the bodies and loads in the system. There are three ways that the equations of motion can be described for Symbolic System: [1] Explicit form where the kinematics and dynamics are combined x' = F_1(x, t, r, p) [2] Implicit form where the kinematics and dynamics are combined M_2(x, p) x' = F_2(x, t, r, p) [3] Implicit form where the kinematics and dynamics are separate M_3(q, p) u' = F_3(q, u, t, r, p) q' = G(q, u, t, r, p) where x : states, e.g. [q, u] t : time r : specified (exogenous) inputs p : constants q : generalized coordinates u : generalized speeds F_1 : right hand side of the combined equations in explicit form F_2 : right hand side of the combined equations in implicit form F_3 : right hand side of the dynamical equations in implicit form M_2 : mass matrix of the combined equations in implicit form M_3 : mass matrix of the dynamical equations in implicit form G : right hand side of the kinematical differential equations Parameters ========== coord_states : ordered iterable of functions of time This input will either be a collection of the coordinates or states of the system depending on whether or not the speeds are also given. If speeds are specified this input will be assumed to be the coordinates otherwise this input will be assumed to be the states. right_hand_side : Matrix This variable is the right hand side of the equations of motion in any of the forms. The specific form will be assumed depending on whether a mass matrix or coordinate derivatives are given. speeds : ordered iterable of functions of time, optional This is a collection of the generalized speeds of the system. If given it will be assumed that the first argument (coord_states) will represent the generalized coordinates of the system. mass_matrix : Matrix, optional The matrix of the implicit forms of the equations of motion (forms [2] and [3]). The distinction between the forms is determined by whether or not the coordinate derivatives are passed in. If they are given form [3] will be assumed otherwise form [2] is assumed. coordinate_derivatives : Matrix, optional The right hand side of the kinematical equations in explicit form. If given it will be assumed that the equations of motion are being entered in form [3]. alg_con : Iterable, optional The indexes of the rows in the equations of motion that contain algebraic constraints instead of differential equations. If the equations are input in form [3], it will be assumed the indexes are referencing the mass_matrix/right_hand_side combination and not the coordinate_derivatives. output_eqns : Dictionary, optional Any output equations that are desired to be tracked are stored in a dictionary where the key corresponds to the name given for the specific equation and the value is the equation itself in symbolic form coord_idxs : Iterable, optional If coord_states corresponds to the states rather than the coordinates this variable will tell SymbolicSystem which indexes of the states correspond to generalized coordinates. speed_idxs : Iterable, optional If coord_states corresponds to the states rather than the coordinates this variable will tell SymbolicSystem which indexes of the states correspond to generalized speeds. bodies : iterable of Body/Rigidbody objects, optional Iterable containing the bodies of the system loads : iterable of load instances (described below), optional Iterable containing the loads of the system where forces are given by (point of application, force vector) and torques are given by (reference frame acting upon, torque vector). Ex [(point, force), (ref_frame, torque)] Attributes ========== coordinates : Matrix, shape(n, 1) This is a matrix containing the generalized coordinates of the system speeds : Matrix, shape(m, 1) This is a matrix containing the generalized speeds of the system states : Matrix, shape(o, 1) This is a matrix containing the state variables of the system alg_con : List This list contains the indices of the algebraic constraints in the combined equations of motion. The presence of these constraints requires that a DAE solver be used instead of an ODE solver. If the system is given in form [3] the alg_con variable will be adjusted such that it is a representation of the combined kinematics and dynamics thus make sure it always matches the mass matrix entered. dyn_implicit_mat : Matrix, shape(m, m) This is the M matrix in form [3] of the equations of motion (the mass matrix or generalized inertia matrix of the dynamical equations of motion in implicit form). dyn_implicit_rhs : Matrix, shape(m, 1) This is the F vector in form [3] of the equations of motion (the right hand side of the dynamical equations of motion in implicit form). comb_implicit_mat : Matrix, shape(o, o) This is the M matrix in form [2] of the equations of motion. This matrix contains a block diagonal structure where the top left block (the first rows) represent the matrix in the implicit form of the kinematical equations and the bottom right block (the last rows) represent the matrix in the implicit form of the dynamical equations. comb_implicit_rhs : Matrix, shape(o, 1) This is the F vector in form [2] of the equations of motion. The top part of the vector represents the right hand side of the implicit form of the kinemaical equations and the bottom of the vector represents the right hand side of the implicit form of the dynamical equations of motion. comb_explicit_rhs : Matrix, shape(o, 1) This vector represents the right hand side of the combined equations of motion in explicit form (form [1] from above). kin_explicit_rhs : Matrix, shape(m, 1) This is the right hand side of the explicit form of the kinematical equations of motion as can be seen in form [3] (the G matrix). output_eqns : Dictionary If output equations were given they are stored in a dictionary where the key corresponds to the name given for the specific equation and the value is the equation itself in symbolic form bodies : Tuple If the bodies in the system were given they are stored in a tuple for future access loads : Tuple If the loads in the system were given they are stored in a tuple for future access. This includes forces and torques where forces are given by (point of application, force vector) and torques are given by (reference frame acted upon, torque vector). Example ======= As a simple example, the dynamics of a simple pendulum will be input into a SymbolicSystem object manually. First some imports will be needed and then symbols will be set up for the length of the pendulum (l), mass at the end of the pendulum (m), and a constant for gravity (g). :: >>> from sympy import Matrix, sin, symbols >>> from sympy.physics.mechanics import dynamicsymbols, SymbolicSystem >>> l, m, g = symbols('l m g') The system will be defined by an angle of theta from the vertical and a generalized speed of omega will be used where omega = theta_dot. :: >>> theta, omega = dynamicsymbols('theta omega') Now the equations of motion are ready to be formed and passed to the SymbolicSystem object. :: >>> kin_explicit_rhs = Matrix([omega]) >>> dyn_implicit_mat = Matrix([l**2 * m]) >>> dyn_implicit_rhs = Matrix([-g * l * m * sin(theta)]) >>> symsystem = SymbolicSystem([theta], dyn_implicit_rhs, [omega], ... dyn_implicit_mat) Notes ===== m : number of generalized speeds n : number of generalized coordinates o : number of states """ def __init__(self, coord_states, right_hand_side, speeds=None, mass_matrix=None, coordinate_derivatives=None, alg_con=None, output_eqns={}, coord_idxs=None, speed_idxs=None, bodies=None, loads=None): """Initializes a SymbolicSystem object""" # Extract information on speeds, coordinates and states if speeds is None: self._states = Matrix(coord_states) if coord_idxs is None: self._coordinates = None else: coords = [coord_states[i] for i in coord_idxs] self._coordinates = Matrix(coords) if speed_idxs is None: self._speeds = None else: speeds_inter = [coord_states[i] for i in speed_idxs] self._speeds = Matrix(speeds_inter) else: self._coordinates = Matrix(coord_states) self._speeds = Matrix(speeds) self._states = self._coordinates.col_join(self._speeds) # Extract equations of motion form if coordinate_derivatives is not None: self._kin_explicit_rhs = coordinate_derivatives self._dyn_implicit_rhs = right_hand_side self._dyn_implicit_mat = mass_matrix self._comb_implicit_rhs = None self._comb_implicit_mat = None self._comb_explicit_rhs = None elif mass_matrix is not None: self._kin_explicit_rhs = None self._dyn_implicit_rhs = None self._dyn_implicit_mat = None self._comb_implicit_rhs = right_hand_side self._comb_implicit_mat = mass_matrix self._comb_explicit_rhs = None else: self._kin_explicit_rhs = None self._dyn_implicit_rhs = None self._dyn_implicit_mat = None self._comb_implicit_rhs = None self._comb_implicit_mat = None self._comb_explicit_rhs = right_hand_side # Set the remainder of the inputs as instance attributes if alg_con is not None and coordinate_derivatives is not None: alg_con = [i + len(coordinate_derivatives) for i in alg_con] self._alg_con = alg_con self.output_eqns = output_eqns # Change the body and loads iterables to tuples if they are not tuples # already if type(bodies) != tuple and bodies is not None: bodies = tuple(bodies) if type(loads) != tuple and loads is not None: loads = tuple(loads) self._bodies = bodies self._loads = loads @property def coordinates(self): """Returns the column matrix of the generalized coordinates""" if self._coordinates is None: raise AttributeError("The coordinates were not specified.") else: return self._coordinates @property def speeds(self): """Returns the column matrix of generalized speeds""" if self._speeds is None: raise AttributeError("The speeds were not specified.") else: return self._speeds @property def states(self): """Returns the column matrix of the state variables""" return self._states @property def alg_con(self): """Returns a list with the indices of the rows containing algebraic constraints in the combined form of the equations of motion""" return self._alg_con @property def dyn_implicit_mat(self): """Returns the matrix, M, corresponding to the dynamic equations in implicit form, M x' = F, where the kinematical equations are not included""" if self._dyn_implicit_mat is None: raise AttributeError("dyn_implicit_mat is not specified for " "equations of motion form [1] or [2].") else: return self._dyn_implicit_mat @property def dyn_implicit_rhs(self): """Returns the column matrix, F, corresponding to the dynamic equations in implicit form, M x' = F, where the kinematical equations are not included""" if self._dyn_implicit_rhs is None: raise AttributeError("dyn_implicit_rhs is not specified for " "equations of motion form [1] or [2].") else: return self._dyn_implicit_rhs @property def comb_implicit_mat(self): """Returns the matrix, M, corresponding to the equations of motion in implicit form (form [2]), M x' = F, where the kinematical equations are included""" if self._comb_implicit_mat is None: if self._dyn_implicit_mat is not None: num_kin_eqns = len(self._kin_explicit_rhs) num_dyn_eqns = len(self._dyn_implicit_rhs) zeros1 = zeros(num_kin_eqns, num_dyn_eqns) zeros2 = zeros(num_dyn_eqns, num_kin_eqns) inter1 = eye(num_kin_eqns).row_join(zeros1) inter2 = zeros2.row_join(self._dyn_implicit_mat) self._comb_implicit_mat = inter1.col_join(inter2) return self._comb_implicit_mat else: raise AttributeError("comb_implicit_mat is not specified for " "equations of motion form [1].") else: return self._comb_implicit_mat @property def comb_implicit_rhs(self): """Returns the column matrix, F, corresponding to the equations of motion in implicit form (form [2]), M x' = F, where the kinematical equations are included""" if self._comb_implicit_rhs is None: if self._dyn_implicit_rhs is not None: kin_inter = self._kin_explicit_rhs dyn_inter = self._dyn_implicit_rhs self._comb_implicit_rhs = kin_inter.col_join(dyn_inter) return self._comb_implicit_rhs else: raise AttributeError("comb_implicit_mat is not specified for " "equations of motion in form [1].") else: return self._comb_implicit_rhs def compute_explicit_form(self): """If the explicit right hand side of the combined equations of motion is to provided upon initialization, this method will calculate it. This calculation can potentially take awhile to compute.""" if self._comb_explicit_rhs is not None: raise AttributeError("comb_explicit_rhs is already formed.") else: try: inter1 = self.kin_explicit_rhs inter2 = self._dyn_implicit_mat.LUsolve(self._dyn_implicit_rhs) out = inter1.col_join(inter2) except AttributeError: out = self._comb_implicit_mat.LUsolve(self._comb_implicit_rhs) self._comb_explicit_rhs = out @property def comb_explicit_rhs(self): """Returns the right hand side of the equations of motion in explicit form, x' = F, where the kinematical equations are included""" if self._comb_explicit_rhs is None: raise AttributeError("Please run .combute_explicit_form before " "attempting to access comb_explicit_rhs.") else: return self._comb_explicit_rhs @property def kin_explicit_rhs(self): """Returns the right hand side of the kinematical equations in explicit form, q' = G""" if self._kin_explicit_rhs is None: raise AttributeError("kin_explicit_rhs is not specified for " "equations of motion form [1] or [2].") else: return self._kin_explicit_rhs 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 constant_symbols(self): """Returns a column matrix containing all of the symbols in the system that do not 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[:]) constants = set() for expr in eom_expressions: constants = constants.union(expr.free_symbols) constants.remove(dynamicsymbols._t) return tuple(constants) @property def bodies(self): """Returns the bodies in the system""" if self._bodies is None: raise AttributeError("bodies were not specified for the system.") else: return self._bodies @property def loads(self): """Returns the loads in the system""" if self._loads is None: raise AttributeError("loads were not specified for the system.") else: return self._loads
class LagrangesMethod(object): """Lagrange's method object. This object generates the equations of motion in a two step procedure. The first step involves the initialization of LagrangesMethod by supplying the Lagrangian and the generalized coordinates, at the bare minimum. If there are any constraint equations, they can be supplied as keyword arguments. The Lagrange multipliers are automatically generated and are equal in number to the constraint equations. Similarly any non-conservative forces can be supplied in an iterable (as described below and also shown in the example) along with a ReferenceFrame. This is also discussed further in the __init__ method. Attributes ========== q, u : Matrix Matrices of the generalized coordinates and speeds forcelist : iterable Iterable of (Point, vector) or (ReferenceFrame, vector) tuples describing the forces on the system. bodies : iterable Iterable containing the rigid bodies and particles of the system. mass_matrix : Matrix The system's mass matrix forcing : Matrix The system's forcing vector mass_matrix_full : Matrix The "mass matrix" for the qdot's, qdoubledot's, and the lagrange multipliers (lam) forcing_full : Matrix The forcing vector for the qdot's, qdoubledot's and lagrange multipliers (lam) Examples ======== This is a simple example for a one degree of freedom translational spring-mass-damper. In this example, we first need to do the kinematics. This involves creating generalized coordinates and their derivatives. Then we create a point and set its velocity in a frame. >>> from sympy.physics.mechanics import LagrangesMethod, Lagrangian >>> from sympy.physics.mechanics import ReferenceFrame, Particle, Point >>> from sympy.physics.mechanics import dynamicsymbols, kinetic_energy >>> from sympy import symbols >>> q = dynamicsymbols('q') >>> qd = dynamicsymbols('q', 1) >>> m, k, b = symbols('m k b') >>> N = ReferenceFrame('N') >>> P = Point('P') >>> P.set_vel(N, qd * N.x) We need to then prepare the information as required by LagrangesMethod to generate equations of motion. First we create the Particle, which has a point attached to it. Following this the lagrangian is created from the kinetic and potential energies. Then, an iterable of nonconservative forces/torques must be constructed, where each item is a (Point, Vector) or (ReferenceFrame, Vector) tuple, with the Vectors representing the nonconservative forces or torques. >>> Pa = Particle('Pa', P, m) >>> Pa.potential_energy = k * q**2 / 2.0 >>> L = Lagrangian(N, Pa) >>> fl = [(P, -b * qd * N.x)] Finally we can generate the equations of motion. First we create the LagrangesMethod object. To do this one must supply the Lagrangian, and the generalized coordinates. The constraint equations, the forcelist, and the inertial frame may also be provided, if relevant. Next we generate Lagrange's equations of motion, such that: Lagrange's equations of motion = 0. We have the equations of motion at this point. >>> l = LagrangesMethod(L, [q], forcelist = fl, frame = N) >>> print(l.form_lagranges_equations()) Matrix([[b*Derivative(q(t), t) + 1.0*k*q(t) + m*Derivative(q(t), t, t)]]) We can also solve for the states using the 'rhs' method. >>> print(l.rhs()) Matrix([[Derivative(q(t), t)], [(-b*Derivative(q(t), t) - 1.0*k*q(t))/m]]) Please refer to the docstrings on each method for more details. """ def __init__(self, Lagrangian, qs, forcelist=None, bodies=None, frame=None, hol_coneqs=None, nonhol_coneqs=None): """Supply the following for the initialization of LagrangesMethod Lagrangian : Sympifyable qs : array_like The generalized coordinates hol_coneqs : array_like, optional The holonomic constraint equations nonhol_coneqs : array_like, optional The nonholonomic constraint equations forcelist : iterable, optional Takes an iterable of (Point, Vector) or (ReferenceFrame, Vector) tuples which represent the force at a point or torque on a frame. This feature is primarily to account for the nonconservative forces and/or moments. bodies : iterable, optional Takes an iterable containing the rigid bodies and particles of the system. frame : ReferenceFrame, optional Supply the inertial frame. This is used to determine the generalized forces due to non-conservative forces. """ self._L = Matrix([sympify(Lagrangian)]) self.eom = None self._m_cd = Matrix() # Mass Matrix of differentiated coneqs self._m_d = Matrix() # Mass Matrix of dynamic equations self._f_cd = Matrix() # Forcing part of the diff coneqs self._f_d = Matrix() # Forcing part of the dynamic equations self.lam_coeffs = Matrix() # The coeffecients of the multipliers forcelist = forcelist if forcelist else [] if not iterable(forcelist): raise TypeError('Force pairs must be supplied in an iterable.') self._forcelist = forcelist if frame and not isinstance(frame, ReferenceFrame): raise TypeError('frame must be a valid ReferenceFrame') self._bodies = bodies self.inertial = frame self.lam_vec = Matrix() self._term1 = Matrix() self._term2 = Matrix() self._term3 = Matrix() self._term4 = Matrix() # Creating the qs, qdots and qdoubledots if not iterable(qs): raise TypeError('Generalized coordinates must be an iterable') self._q = Matrix(qs) self._qdots = self.q.diff(dynamicsymbols._t) self._qdoubledots = self._qdots.diff(dynamicsymbols._t) mat_build = lambda x: Matrix(x) if x else Matrix() hol_coneqs = mat_build(hol_coneqs) nonhol_coneqs = mat_build(nonhol_coneqs) self.coneqs = Matrix([hol_coneqs.diff(dynamicsymbols._t), nonhol_coneqs]) self._hol_coneqs = hol_coneqs 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) for i, qd in enumerate(qds): flist = zip(*_f_list_parser(self.forcelist, N)) 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 @property def mass_matrix(self): """Returns the mass matrix, which is augmented by the Lagrange multipliers, if necessary. If the system is described by 'n' generalized coordinates and there are no constraint equations then an n X n matrix is returned. If there are 'n' generalized coordinates and 'm' constraint equations have been supplied during initialization then an n X (n+m) matrix is returned. The (n + m - 1)th and (n + m)th columns contain the coefficients of the Lagrange multipliers. """ if self.eom is None: raise ValueError('Need to compute the equations of motion first') if self.coneqs: return (self._m_d).row_join(self.lam_coeffs.T) else: return self._m_d @property def mass_matrix_full(self): """Augments the coefficients of qdots to the mass_matrix.""" if self.eom is None: raise ValueError('Need to compute the equations of motion first') n = len(self.q) m = len(self.coneqs) row1 = eye(n).row_join(zeros(n, n + m)) row2 = zeros(n, n).row_join(self.mass_matrix) if self.coneqs: row3 = zeros(m, n).row_join(self._m_cd).row_join(zeros(m, m)) return row1.col_join(row2).col_join(row3) else: return row1.col_join(row2) @property def forcing(self): """Returns the forcing vector from 'lagranges_equations' method.""" if self.eom is None: raise ValueError('Need to compute the equations of motion first') return self._f_d @property def forcing_full(self): """Augments qdots to the forcing vector above.""" if self.eom is None: raise ValueError('Need to compute the equations of motion first') if self.coneqs: return self._qdots.col_join(self.forcing).col_join(self._f_cd) else: return self._qdots.col_join(self.forcing) 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 linearize(self, q_ind=None, qd_ind=None, q_dep=None, qd_dep=None, **kwargs): """Linearize the equations of motion about a symbolic operating point. If kwarg A_and_B is False (default), returns M, A, B, r for the linearized form, M*[q', u']^T = A*[q_ind, u_ind]^T + B*r. If kwarg A_and_B is True, returns A, B, r for the linearized form dx = A*x + B*r, where x = [q_ind, u_ind]^T. Note that this is computationally intensive if there are many symbolic parameters. For this reason, it may be more desirable to use the default A_and_B=False, returning M, A, and B. Values may then be substituted in to these matrices, and the state space form found as A = P.T*M.inv()*A, B = P.T*M.inv()*B, where P = Linearizer.perm_mat. In both cases, r is found as all dynamicsymbols in the equations of motion that are not part of q, u, q', or u'. They are sorted in canonical form. The operating points may be also entered using the ``op_point`` kwarg. This takes a dictionary of {symbol: value}, or a an iterable of such dictionaries. The values may be numberic or symbolic. The more values you can specify beforehand, the faster this computation will run. For more documentation, please see the ``Linearizer`` class.""" linearizer = self.to_linearizer(q_ind, qd_ind, q_dep, qd_dep) result = linearizer.linearize(**kwargs) return result + (linearizer.r,) def solve_multipliers(self, op_point=None, sol_type='dict'): """Solves for the values of the lagrange multipliers symbolically at the specified operating point Parameters ========== op_point : dict or iterable of dicts, optional Point at which to solve at. The operating point is specified as a dictionary or iterable of dictionaries of {symbol: value}. The value may be numeric or symbolic itself. sol_type : str, optional Solution return type. Valid options are: - 'dict': A dict of {symbol : value} (default) - 'Matrix': An ordered column matrix of the solution """ # Determine number of multipliers k = len(self.lam_vec) if k == 0: raise ValueError("System has no lagrange multipliers to solve for.") # Compose dict of operating conditions if isinstance(op_point, dict): op_point_dict = op_point elif iterable(op_point): op_point_dict = {} for op in op_point: op_point_dict.update(op) elif op_point is None: op_point_dict = {} else: raise TypeError("op_point must be either a dictionary or an " "iterable of dictionaries.") # Compose the system to be solved mass_matrix = self.mass_matrix.col_join((-self.lam_coeffs.row_join( zeros(k, k)))) force_matrix = self.forcing.col_join(self._f_cd) # Sub in the operating point mass_matrix = msubs(mass_matrix, op_point_dict) force_matrix = msubs(force_matrix, op_point_dict) # Solve for the multipliers sol_list = mass_matrix.LUsolve(-force_matrix)[-k:] if sol_type == 'dict': return dict(zip(self.lam_vec, sol_list)) elif sol_type == 'Matrix': return Matrix(sol_list) else: raise ValueError("Unknown sol_type {:}.".format(sol_type)) def rhs(self, inv_method=None, **kwargs): """Returns equations that can be solved numerically Parameters ========== inv_method : str The specific sympy inverse matrix calculation method to use. For a list of valid methods, see :meth:`~sympy.matrices.matrices.MatrixBase.inv` """ if inv_method is None: self._rhs = self.mass_matrix_full.LUsolve(self.forcing_full) else: self._rhs = (self.mass_matrix_full.inv(inv_method, try_block_diag=True) * self.forcing_full) return self._rhs @property def q(self): return self._q @property def u(self): return self._qdots @property def bodies(self): return self._bodies @property def forcelist(self): return self._forcelist
def orient(self, parent, rot_type, amounts, rot_order=''): """Sets the orientation of this reference frame relative to another (parent) reference frame. Parameters ========== parent : ReferenceFrame Reference frame that this reference frame will be rotated relative to. rot_type : str The method used to generate the direction cosine matrix. Supported methods are: - ``'Axis'``: simple rotations about a single common axis - ``'DCM'``: for setting the direction cosine matrix directly - ``'Body'``: three successive rotations about new intermediate axes, also called "Euler and Tait-Bryan angles" - ``'Space'``: three successive rotations about the parent frames' unit vectors - ``'Quaternion'``: rotations defined by four parameters which result in a singularity free direction cosine matrix amounts : Expressions defining the rotation angles or direction cosine matrix. These must match the ``rot_type``. See examples below for details. The input types are: - ``'Axis'``: 2-tuple (expr/sym/func, Vector) - ``'DCM'``: Matrix, shape(3,3) - ``'Body'``: 3-tuple of expressions, symbols, or functions - ``'Space'``: 3-tuple of expressions, symbols, or functions - ``'Quaternion'``: 4-tuple of expressions, symbols, or functions rot_order : str or int, optional If applicable, the order of the successive of rotations. The string ``'123'`` and integer ``123`` are equivalent, for example. Required for ``'Body'`` and ``'Space'``. Examples ======== Setup variables for the examples: >>> from sympy import symbols >>> from sympy.physics.vector import ReferenceFrame >>> q0, q1, q2, q3 = symbols('q0 q1 q2 q3') >>> N = ReferenceFrame('N') >>> B = ReferenceFrame('B') >>> B1 = ReferenceFrame('B') >>> B2 = ReferenceFrame('B2') Axis ---- ``rot_type='Axis'`` creates a direction cosine matrix defined by a simple rotation about a single axis fixed in both reference frames. This is a rotation about an arbitrary, non-time-varying axis by some angle. The axis is supplied as a Vector. This is how simple rotations are defined. >>> B.orient(N, 'Axis', (q1, N.x)) The ``orient()`` method generates a direction cosine matrix and its transpose which defines the orientation of B relative to N and vice versa. Once orient is called, ``dcm()`` outputs the appropriate direction cosine matrix. >>> B.dcm(N) Matrix([ [1, 0, 0], [0, cos(q1), sin(q1)], [0, -sin(q1), cos(q1)]]) The following two lines show how the sense of the rotation can be defined. Both lines produce the same result. >>> B.orient(N, 'Axis', (q1, -N.x)) >>> B.orient(N, 'Axis', (-q1, N.x)) The axis does not have to be defined by a unit vector, it can be any vector in the parent frame. >>> B.orient(N, 'Axis', (q1, N.x + 2 * N.y)) DCM --- The direction cosine matrix can be set directly. The orientation of a frame A can be set to be the same as the frame B above like so: >>> B.orient(N, 'Axis', (q1, N.x)) >>> A = ReferenceFrame('A') >>> A.orient(N, 'DCM', N.dcm(B)) >>> A.dcm(N) Matrix([ [1, 0, 0], [0, cos(q1), sin(q1)], [0, -sin(q1), cos(q1)]]) **Note carefully that** ``N.dcm(B)`` **was passed into** ``orient()`` **for** ``A.dcm(N)`` **to match** ``B.dcm(N)``. Body ---- ``rot_type='Body'`` rotates this reference frame relative to the provided reference frame by rotating through three successive simple rotations. Each subsequent axis of rotation is about the "body fixed" unit vectors of the new intermediate reference frame. This type of rotation is also referred to rotating through the `Euler and Tait-Bryan Angles <https://en.wikipedia.org/wiki/Euler_angles>`_. For example, the classic Euler Angle rotation can be done by: >>> B.orient(N, 'Body', (q1, q2, q3), 'XYX') >>> B.dcm(N) Matrix([ [ cos(q2), sin(q1)*sin(q2), -sin(q2)*cos(q1)], [sin(q2)*sin(q3), -sin(q1)*sin(q3)*cos(q2) + cos(q1)*cos(q3), sin(q1)*cos(q3) + sin(q3)*cos(q1)*cos(q2)], [sin(q2)*cos(q3), -sin(q1)*cos(q2)*cos(q3) - sin(q3)*cos(q1), -sin(q1)*sin(q3) + cos(q1)*cos(q2)*cos(q3)]]) This rotates B relative to N through ``q1`` about ``N.x``, then rotates B again through q2 about B.y, and finally through q3 about B.x. It is equivalent to: >>> B1.orient(N, 'Axis', (q1, N.x)) >>> B2.orient(B1, 'Axis', (q2, B1.y)) >>> B.orient(B2, 'Axis', (q3, B2.x)) >>> B.dcm(N) Matrix([ [ cos(q2), sin(q1)*sin(q2), -sin(q2)*cos(q1)], [sin(q2)*sin(q3), -sin(q1)*sin(q3)*cos(q2) + cos(q1)*cos(q3), sin(q1)*cos(q3) + sin(q3)*cos(q1)*cos(q2)], [sin(q2)*cos(q3), -sin(q1)*cos(q2)*cos(q3) - sin(q3)*cos(q1), -sin(q1)*sin(q3) + cos(q1)*cos(q2)*cos(q3)]]) Acceptable rotation orders are of length 3, expressed in as a string ``'XYZ'`` or ``'123'`` or integer ``123``. Rotations about an axis twice in a row are prohibited. >>> B.orient(N, 'Body', (q1, q2, 0), 'ZXZ') >>> B.orient(N, 'Body', (q1, q2, 0), '121') >>> B.orient(N, 'Body', (q1, q2, q3), 123) Space ----- ``rot_type='Space'`` also rotates the reference frame in three successive simple rotations but the axes of rotation are the "Space-fixed" axes. For example: >>> B.orient(N, 'Space', (q1, q2, q3), '312') >>> B.dcm(N) Matrix([ [ sin(q1)*sin(q2)*sin(q3) + cos(q1)*cos(q3), sin(q1)*cos(q2), sin(q1)*sin(q2)*cos(q3) - sin(q3)*cos(q1)], [-sin(q1)*cos(q3) + sin(q2)*sin(q3)*cos(q1), cos(q1)*cos(q2), sin(q1)*sin(q3) + sin(q2)*cos(q1)*cos(q3)], [ sin(q3)*cos(q2), -sin(q2), cos(q2)*cos(q3)]]) is equivalent to: >>> B1.orient(N, 'Axis', (q1, N.z)) >>> B2.orient(B1, 'Axis', (q2, N.x)) >>> B.orient(B2, 'Axis', (q3, N.y)) >>> B.dcm(N).simplify() # doctest: +SKIP Matrix([ [ sin(q1)*sin(q2)*sin(q3) + cos(q1)*cos(q3), sin(q1)*cos(q2), sin(q1)*sin(q2)*cos(q3) - sin(q3)*cos(q1)], [-sin(q1)*cos(q3) + sin(q2)*sin(q3)*cos(q1), cos(q1)*cos(q2), sin(q1)*sin(q3) + sin(q2)*cos(q1)*cos(q3)], [ sin(q3)*cos(q2), -sin(q2), cos(q2)*cos(q3)]]) It is worth noting that space-fixed and body-fixed rotations are related by the order of the rotations, i.e. the reverse order of body fixed will give space fixed and vice versa. >>> B.orient(N, 'Space', (q1, q2, q3), '231') >>> B.dcm(N) Matrix([ [cos(q1)*cos(q2), sin(q1)*sin(q3) + sin(q2)*cos(q1)*cos(q3), -sin(q1)*cos(q3) + sin(q2)*sin(q3)*cos(q1)], [ -sin(q2), cos(q2)*cos(q3), sin(q3)*cos(q2)], [sin(q1)*cos(q2), sin(q1)*sin(q2)*cos(q3) - sin(q3)*cos(q1), sin(q1)*sin(q2)*sin(q3) + cos(q1)*cos(q3)]]) >>> B.orient(N, 'Body', (q3, q2, q1), '132') >>> B.dcm(N) Matrix([ [cos(q1)*cos(q2), sin(q1)*sin(q3) + sin(q2)*cos(q1)*cos(q3), -sin(q1)*cos(q3) + sin(q2)*sin(q3)*cos(q1)], [ -sin(q2), cos(q2)*cos(q3), sin(q3)*cos(q2)], [sin(q1)*cos(q2), sin(q1)*sin(q2)*cos(q3) - sin(q3)*cos(q1), sin(q1)*sin(q2)*sin(q3) + cos(q1)*cos(q3)]]) Quaternion ---------- ``rot_type='Quaternion'`` orients the reference frame using quaternions. Quaternion rotation is defined as a finite rotation about lambda, a unit vector, by an amount theta. This orientation is described by four parameters: - ``q0 = cos(theta/2)`` - ``q1 = lambda_x sin(theta/2)`` - ``q2 = lambda_y sin(theta/2)`` - ``q3 = lambda_z sin(theta/2)`` This type does not need a ``rot_order``. >>> B.orient(N, 'Quaternion', (q0, q1, q2, q3)) >>> B.dcm(N) Matrix([ [q0**2 + q1**2 - q2**2 - q3**2, 2*q0*q3 + 2*q1*q2, -2*q0*q2 + 2*q1*q3], [ -2*q0*q3 + 2*q1*q2, q0**2 - q1**2 + q2**2 - q3**2, 2*q0*q1 + 2*q2*q3], [ 2*q0*q2 + 2*q1*q3, -2*q0*q1 + 2*q2*q3, q0**2 - q1**2 - q2**2 + q3**2]]) """ from sympy.physics.vector.functions import dynamicsymbols _check_frame(parent) # Allow passing a rotation matrix manually. if rot_type == 'DCM': # When rot_type == 'DCM', then amounts must be a Matrix type object # (e.g. sympy.matrices.dense.MutableDenseMatrix). if not isinstance(amounts, MatrixBase): raise TypeError("Amounts must be a sympy Matrix type object.") else: amounts = list(amounts) for i, v in enumerate(amounts): if not isinstance(v, Vector): amounts[i] = sympify(v) def _rot(axis, angle): """DCM for simple axis 1,2,or 3 rotations. """ if axis == 1: return Matrix([[1, 0, 0], [0, cos(angle), -sin(angle)], [0, sin(angle), cos(angle)]]) elif axis == 2: return Matrix([[cos(angle), 0, sin(angle)], [0, 1, 0], [-sin(angle), 0, cos(angle)]]) elif axis == 3: return Matrix([[cos(angle), -sin(angle), 0], [sin(angle), cos(angle), 0], [0, 0, 1]]) approved_orders = ('123', '231', '312', '132', '213', '321', '121', '131', '212', '232', '313', '323', '') # make sure XYZ => 123 and rot_type is in upper case rot_order = translate(str(rot_order), 'XYZxyz', '123123') rot_type = rot_type.upper() if rot_order not in approved_orders: raise TypeError('The supplied order is not an approved type') parent_orient = [] if rot_type == 'AXIS': if not rot_order == '': raise TypeError('Axis orientation takes no rotation order') if not (isinstance(amounts, (list, tuple)) & (len(amounts) == 2)): raise TypeError('Amounts are a list or tuple of length 2') theta = amounts[0] axis = amounts[1] axis = _check_vector(axis) if not axis.dt(parent) == 0: raise ValueError('Axis cannot be time-varying') axis = axis.express(parent).normalize() axis = axis.args[0][0] parent_orient = ((eye(3) - axis * axis.T) * cos(theta) + Matrix([[0, -axis[2], axis[1]], [axis[2], 0, -axis[0]], [-axis[1], axis[0], 0]]) * sin(theta) + axis * axis.T) elif rot_type == 'QUATERNION': if not rot_order == '': raise TypeError( 'Quaternion orientation takes no rotation order') if not (isinstance(amounts, (list, tuple)) & (len(amounts) == 4)): raise TypeError('Amounts are a list or tuple of length 4') q0, q1, q2, q3 = amounts parent_orient = (Matrix([[q0**2 + q1**2 - q2**2 - q3**2, 2 * (q1 * q2 - q0 * q3), 2 * (q0 * q2 + q1 * q3)], [2 * (q1 * q2 + q0 * q3), q0**2 - q1**2 + q2**2 - q3**2, 2 * (q2 * q3 - q0 * q1)], [2 * (q1 * q3 - q0 * q2), 2 * (q0 * q1 + q2 * q3), q0**2 - q1**2 - q2**2 + q3**2]])) elif rot_type == 'BODY': if not (len(amounts) == 3 & len(rot_order) == 3): raise TypeError('Body orientation takes 3 values & 3 orders') a1 = int(rot_order[0]) a2 = int(rot_order[1]) a3 = int(rot_order[2]) parent_orient = (_rot(a1, amounts[0]) * _rot(a2, amounts[1]) * _rot(a3, amounts[2])) elif rot_type == 'SPACE': if not (len(amounts) == 3 & len(rot_order) == 3): raise TypeError('Space orientation takes 3 values & 3 orders') a1 = int(rot_order[0]) a2 = int(rot_order[1]) a3 = int(rot_order[2]) parent_orient = (_rot(a3, amounts[2]) * _rot(a2, amounts[1]) * _rot(a1, amounts[0])) elif rot_type == 'DCM': parent_orient = amounts else: raise NotImplementedError('That is not an implemented rotation') # Reset the _dcm_cache of this frame, and remove it from the # _dcm_caches of the frames it is linked to. Also remove it from the # _dcm_dict of its parent frames = self._dcm_cache.keys() dcm_dict_del = [] dcm_cache_del = [] for frame in frames: if frame in self._dcm_dict: dcm_dict_del += [frame] dcm_cache_del += [frame] for frame in dcm_dict_del: del frame._dcm_dict[self] for frame in dcm_cache_del: del frame._dcm_cache[self] # Add the dcm relationship to _dcm_dict self._dcm_dict = self._dlist[0] = {} self._dcm_dict.update({parent: parent_orient.T}) parent._dcm_dict.update({self: parent_orient}) # Also update the dcm cache after resetting it self._dcm_cache = {} self._dcm_cache.update({parent: parent_orient.T}) parent._dcm_cache.update({self: parent_orient}) if rot_type == 'QUATERNION': t = dynamicsymbols._t q0, q1, q2, q3 = amounts q0d = diff(q0, t) q1d = diff(q1, t) q2d = diff(q2, t) q3d = diff(q3, t) w1 = 2 * (q1d * q0 + q2d * q3 - q3d * q2 - q0d * q1) w2 = 2 * (q2d * q0 + q3d * q1 - q1d * q3 - q0d * q2) w3 = 2 * (q3d * q0 + q1d * q2 - q2d * q1 - q0d * q3) wvec = Vector([(Matrix([w1, w2, w3]), self)]) elif rot_type == 'AXIS': thetad = (amounts[0]).diff(dynamicsymbols._t) wvec = thetad * amounts[1].express(parent).normalize() elif rot_type == 'DCM': wvec = self._w_diff_dcm(parent) else: try: from sympy.polys.polyerrors import CoercionFailed from sympy.physics.vector.functions import kinematic_equations q1, q2, q3 = amounts u1, u2, u3 = symbols('u1, u2, u3', cls=Dummy) templist = kinematic_equations([u1, u2, u3], [q1, q2, q3], rot_type, rot_order) templist = [expand(i) for i in templist] td = solve(templist, [u1, u2, u3]) u1 = expand(td[u1]) u2 = expand(td[u2]) u3 = expand(td[u3]) wvec = u1 * self.x + u2 * self.y + u3 * self.z except (CoercionFailed, AssertionError): wvec = self._w_diff_dcm(parent) self._ang_vel_dict.update({parent: wvec}) parent._ang_vel_dict.update({self: -wvec}) self._var_dict = {}
class KanesMethod(object): """Kane's method object. This object is used to do the "book-keeping" as you go through and form equations of motion in the way Kane presents in: Kane, T., Levinson, D. Dynamics Theory and Applications. 1985 McGraw-Hill The attributes are for equations in the form [M] udot = forcing. Attributes ========== q, u : Matrix Matrices of the generalized coordinates and speeds bodylist : iterable Iterable of Point and RigidBody objects in the system. forcelist : iterable Iterable of (Point, vector) or (ReferenceFrame, vector) tuples describing the forces on the system. auxiliary : Matrix If applicable, the set of auxiliary Kane's equations used to solve for non-contributing forces. mass_matrix : Matrix The system's mass matrix forcing : Matrix The system's forcing vector mass_matrix_full : Matrix The "mass matrix" for the u's and q's forcing_full : Matrix The "forcing vector" for the u's and q's Examples ======== This is a simple example for a one degree of freedom translational spring-mass-damper. In this example, we first need to do the kinematics. This involves creating generalized speeds and coordinates and their derivatives. Then we create a point and set its velocity in a frame. >>> from sympy import symbols >>> from sympy.physics.mechanics import dynamicsymbols, ReferenceFrame >>> from sympy.physics.mechanics import Point, Particle, KanesMethod >>> q, u = dynamicsymbols('q u') >>> qd, ud = dynamicsymbols('q u', 1) >>> m, c, k = symbols('m c k') >>> N = ReferenceFrame('N') >>> P = Point('P') >>> P.set_vel(N, u * N.x) Next we need to arrange/store information in the way that KanesMethod requires. The kinematic differential equations need to be stored in a dict. A list of forces/torques must be constructed, where each entry in the list is a (Point, Vector) or (ReferenceFrame, Vector) tuple, where the Vectors represent the Force or Torque. Next a particle needs to be created, and it needs to have a point and mass assigned to it. Finally, a list of all bodies and particles needs to be created. >>> kd = [qd - u] >>> FL = [(P, (-k * q - c * u) * N.x)] >>> pa = Particle('pa', P, m) >>> BL = [pa] Finally we can generate the equations of motion. First we create the KanesMethod object and supply an inertial frame, coordinates, generalized speeds, and the kinematic differential equations. Additional quantities such as configuration and motion constraints, dependent coordinates and speeds, and auxiliary speeds are also supplied here (see the online documentation). Next we form FR* and FR to complete: Fr + Fr* = 0. We have the equations of motion at this point. It makes sense to rearrnge them though, so we calculate the mass matrix and the forcing terms, for E.o.M. in the form: [MM] udot = forcing, where MM is the mass matrix, udot is a vector of the time derivatives of the generalized speeds, and forcing is a vector representing "forcing" terms. >>> KM = KanesMethod(N, q_ind=[q], u_ind=[u], kd_eqs=kd) >>> (fr, frstar) = KM.kanes_equations(BL, FL) >>> MM = KM.mass_matrix >>> forcing = KM.forcing >>> rhs = MM.inv() * forcing >>> rhs Matrix([[(-c*u(t) - k*q(t))/m]]) >>> KM.linearize(A_and_B=True, new_method=True)[0] Matrix([ [ 0, 1], [-k/m, -c/m]]) Please look at the documentation pages for more information on how to perform linearization and how to deal with dependent coordinates & speeds, and how do deal with bringing non-contributing forces into evidence. """ def __init__(self, frame, q_ind, u_ind, kd_eqs=None, q_dependent=None, configuration_constraints=None, u_dependent=None, velocity_constraints=None, acceleration_constraints=None, u_auxiliary=None): """Please read the online documentation. """ if not isinstance(frame, ReferenceFrame): raise TypeError('An intertial ReferenceFrame must be supplied') self._inertial = frame self._fr = None self._frstar = None self._forcelist = None self._bodylist = None self._initialize_vectors(q_ind, q_dependent, u_ind, u_dependent, u_auxiliary) self._initialize_kindiffeq_matrices(kd_eqs) self._initialize_constraint_matrices(configuration_constraints, velocity_constraints, acceleration_constraints) def _initialize_vectors(self, q_ind, q_dep, u_ind, u_dep, u_aux): """Initialize the coordinate and speed vectors.""" none_handler = lambda x: Matrix(x) if x else Matrix() # Initialize generalized coordinates q_dep = none_handler(q_dep) if not iterable(q_ind): raise TypeError('Generalized coordinates must be an iterable.') if not iterable(q_dep): raise TypeError('Dependent coordinates must be an iterable.') q_ind = Matrix(q_ind) self._qdep = q_dep self._q = Matrix([q_ind, q_dep]) self._qdot = self.q.diff(dynamicsymbols._t) # Initialize generalized speeds u_dep = none_handler(u_dep) if not iterable(u_ind): raise TypeError('Generalized speeds must be an iterable.') if not iterable(u_dep): raise TypeError('Dependent speeds must be an iterable.') u_ind = Matrix(u_ind) self._udep = u_dep self._u = Matrix([u_ind, u_dep]) self._udot = self.u.diff(dynamicsymbols._t) self._uaux = none_handler(u_aux) def _initialize_constraint_matrices(self, config, vel, acc): """Initializes constraint matrices.""" # Define vector dimensions o = len(self.u) m = len(self._udep) p = o - m none_handler = lambda x: Matrix(x) if x else Matrix() # Initialize configuration constraints config = none_handler(config) if len(self._qdep) != len(config): raise ValueError('There must be an equal number of dependent ' 'coordinates and configuration constraints.') self._f_h = none_handler(config) # Initialize velocity and acceleration constraints vel = none_handler(vel) acc = none_handler(acc) if len(vel) != m: raise ValueError('There must be an equal number of dependent ' 'speeds and velocity constraints.') if acc and (len(acc) != m): raise ValueError('There must be an equal number of dependent ' 'speeds and acceleration constraints.') if vel: u_zero = dict((i, 0) for i in self.u) udot_zero = dict((i, 0) for i in self._udot) # When calling kanes_equations, another class instance will be # created if auxiliary u's are present. In this case, the # computation of kinetic differential equation matrices will be # skipped as this was computed during the original KanesMethod # object, and the qd_u_map will not be available. if self._qdot_u_map is not None: vel = msubs(vel, self._qdot_u_map) self._f_nh = msubs(vel, u_zero) self._k_nh = (vel - self._f_nh).jacobian(self.u) # If no acceleration constraints given, calculate them. if not acc: self._f_dnh = (self._k_nh.diff(dynamicsymbols._t) * self.u + self._f_nh.diff(dynamicsymbols._t)) self._k_dnh = self._k_nh else: if self._qdot_u_map is not None: acc = msubs(acc, self._qdot_u_map) self._f_dnh = msubs(acc, udot_zero) self._k_dnh = (acc - self._f_dnh).jacobian(self._udot) # Form of non-holonomic constraints is B*u + C = 0. # We partition B into independent and dependent columns: # Ars is then -B_dep.inv() * B_ind, and it relates dependent speeds # to independent speeds as: udep = Ars*uind, neglecting the C term. B_ind = self._k_nh[:, :p] B_dep = self._k_nh[:, p:o] self._Ars = -B_dep.LUsolve(B_ind) else: self._f_nh = Matrix() self._k_nh = Matrix() self._f_dnh = Matrix() self._k_dnh = Matrix() self._Ars = Matrix() def _initialize_kindiffeq_matrices(self, kdeqs): """Initialize the kinematic differential equation matrices.""" if kdeqs: if len(self.q) != len(kdeqs): raise ValueError('There must be an equal number of kinematic ' 'differential equations and coordinates.') kdeqs = Matrix(kdeqs) u = self.u qdot = self._qdot # Dictionaries setting things to zero u_zero = dict((i, 0) for i in u) uaux_zero = dict((i, 0) for i in self._uaux) qdot_zero = dict((i, 0) for i in qdot) f_k = msubs(kdeqs, u_zero, qdot_zero) k_ku = (msubs(kdeqs, qdot_zero) - f_k).jacobian(u) k_kqdot = (msubs(kdeqs, u_zero) - f_k).jacobian(qdot) f_k = k_kqdot.LUsolve(f_k) k_ku = k_kqdot.LUsolve(k_ku) k_kqdot = eye(len(qdot)) self._qdot_u_map = solve_linear_system_LU( Matrix([k_kqdot.T, -(k_ku * u + f_k).T]).T, qdot) self._f_k = msubs(f_k, uaux_zero) self._k_ku = msubs(k_ku, 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() 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_frstar(self, bl): """Form the generalized inertia force.""" if not iterable(bl): raise TypeError('Bodies must be supplied in an iterable.') t = dynamicsymbols._t N = self._inertial # Dicts setting things to zero udot_zero = dict((i, 0) for i in self._udot) uaux_zero = dict((i, 0) for i in self._uaux) uauxdot = [diff(i, t) for i in self._uaux] uauxdot_zero = dict((i, 0) for i in uauxdot) # Dictionary of q' and q'' to u and u' q_ddot_u_map = dict((k.diff(t), v.diff(t)) for (k, v) in self._qdot_u_map.items()) q_ddot_u_map.update(self._qdot_u_map) # Fill up the list of partials: format is a list with num 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. def get_partial_velocity(body): if isinstance(body, RigidBody): vlist = [body.masscenter.vel(N), body.frame.ang_vel_in(N)] elif isinstance(body, Particle): vlist = [body.point.vel(N),] else: raise TypeError('The body list may only contain either ' 'RigidBody or Particle as list elements.') v = [msubs(vel, self._qdot_u_map) for vel in vlist] return partial_velocity(v, self.u, N) partials = [get_partial_velocity(body) for body in bl] # Compute fr_star in two components: # fr_star = -(MM*u' + nonMM) o = len(self.u) MM = zeros(o, o) nonMM = zeros(o, 1) zero_uaux = lambda expr: msubs(expr, uaux_zero) zero_udot_uaux = lambda expr: msubs(msubs(expr, udot_zero), uaux_zero) for i, body in enumerate(bl): if isinstance(body, RigidBody): M = zero_uaux(body.mass) I = zero_uaux(body.central_inertia) vel = zero_uaux(body.masscenter.vel(N)) omega = zero_uaux(body.frame.ang_vel_in(N)) acc = zero_udot_uaux(body.masscenter.acc(N)) inertial_force = (M.diff(t) * vel + M * acc) inertial_torque = zero_uaux((I.dt(body.frame) & omega) + msubs(I & body.frame.ang_acc_in(N), udot_zero) + (omega ^ (I & omega))) for j in range(o): tmp_vel = zero_uaux(partials[i][0][j]) tmp_ang = zero_uaux(I & partials[i][1][j]) 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] else: M = zero_uaux(body.mass) vel = zero_uaux(body.point.vel(N)) acc = zero_udot_uaux(body.point.acc(N)) inertial_force = (M.diff(t) * vel + M * acc) for j in range(o): temp = zero_uaux(partials[i][0][j]) for k in range(o): MM[j, k] += M * (temp & partials[i][0][k]) nonMM[j] += inertial_force & partials[i][0][j] # Compose fr_star out of MM and nonMM MM = zero_uaux(msubs(MM, q_ddot_u_map)) nonMM = msubs(msubs(nonMM, q_ddot_u_map), udot_zero, uauxdot_zero, uaux_zero) fr_star = -(MM * msubs(Matrix(self._udot), uauxdot_zero) + nonMM) # If there are dependent speeds, we need to find fr_star_tilde if self._udep: p = o - len(self._udep) fr_star_ind = fr_star[:p, 0] fr_star_dep = fr_star[p:o, 0] fr_star = fr_star_ind + (self._Ars.T * fr_star_dep) # Apply the same to MM MMi = MM[:p, :] MMd = MM[p:o, :] MM = MMi + (self._Ars.T * MMd) self._bodylist = bl self._frstar = fr_star self._k_d = MM self._f_d = -msubs(self._fr + self._frstar, udot_zero) return fr_star 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 linearize(self, **kwargs): """ Linearize the equations of motion about a symbolic operating point. If kwarg A_and_B is False (default), returns M, A, B, r for the linearized form, M*[q', u']^T = A*[q_ind, u_ind]^T + B*r. If kwarg A_and_B is True, returns A, B, r for the linearized form dx = A*x + B*r, where x = [q_ind, u_ind]^T. Note that this is computationally intensive if there are many symbolic parameters. For this reason, it may be more desirable to use the default A_and_B=False, returning M, A, and B. Values may then be substituted in to these matrices, and the state space form found as A = P.T*M.inv()*A, B = P.T*M.inv()*B, where P = Linearizer.perm_mat. In both cases, r is found as all dynamicsymbols in the equations of motion that are not part of q, u, q', or u'. They are sorted in canonical form. The operating points may be also entered using the ``op_point`` kwarg. This takes a dictionary of {symbol: value}, or a an iterable of such dictionaries. The values may be numberic or symbolic. The more values you can specify beforehand, the faster this computation will run. As part of the deprecation cycle, the new method will not be used unless the kwarg ``new_method`` is set to True. If the kwarg is missing, or set to false, the old linearization method will be used. After next release the need for this kwarg will be removed. For more documentation, please see the ``Linearizer`` class.""" if 'new_method' not in kwargs or not kwargs['new_method']: # User is still using old code. SymPyDeprecationWarning('The linearize class method has changed ' 'to a new interface, the old method is deprecated. To ' 'use the new method, set the kwarg `new_method=True`. ' 'For more information, read the docstring ' 'of `linearize`.').warn() return self._old_linearize() # Remove the new method flag, before passing kwargs to linearize kwargs.pop('new_method') linearizer = self.to_linearizer() result = linearizer.linearize(**kwargs) return result + (linearizer.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(zip(uaux[:] + uauxdot[:], [0] * (len(uaux) + len(uauxdot)))) # Checking for dynamic symbols outside the dynamic differential # equations; throws error if there is. insyms = set(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(msubs(self._f_d, 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 = msubs(f1, subdict) f2 = msubs(f2, subdict) fh = msubs(self._f_h, subdict) fku = msubs(self._k_ku * Matrix(self.u), subdict) fkf = msubs(self._f_k, 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 kanes_equations(self, bodies, loads=None): """ Method to form Kane's equations, Fr + Fr* = 0. Returns (Fr, Fr*). In the case where auxiliary generalized speeds are present (say, s auxiliary speeds, o generalized speeds, and m motion constraints) the length of the returned vectors will be o - m + s in length. The first o - m equations will be the constrained Kane's equations, then the s auxiliary Kane's equations. These auxiliary equations can be accessed with the auxiliary_eqs(). Parameters ========== bodies : iterable An iterable of all RigidBody's and Particle's in the system. A system must have at least one body. loads : iterable Takes in an iterable of (Particle, Vector) or (ReferenceFrame, Vector) tuples which represent the force at a point or torque on a frame. Must be either a non-empty iterable of tuples or None which corresponds to a system with no constraints. """ if (bodies is None and loads != None) or isinstance(bodies[0], tuple): # This switches the order if they use the old way. bodies, loads = loads, bodies SymPyDeprecationWarning(value='The API for kanes_equations() has changed such ' 'that the loads (forces and torques) are now the second argument ' 'and is optional with None being the default.', feature='The kanes_equation() argument order', useinstead='switched argument order to update your code, For example: ' 'kanes_equations(loads, bodies) > kanes_equations(bodies, loads).', issue=10945, deprecated_since_version="1.1").warn() if not self._k_kqdot: raise AttributeError('Create an instance of KanesMethod with ' 'kinematic differential equations to use this method.') fr = self._form_fr(loads) frstar = self._form_frstar(bodies) if self._uaux: if not self._udep: km = KanesMethod(self._inertial, self.q, self._uaux, u_auxiliary=self._uaux) else: km = KanesMethod(self._inertial, self.q, self._uaux, u_auxiliary=self._uaux, u_dependent=self._udep, velocity_constraints=(self._k_nh * self.u + self._f_nh)) km._qdot_u_map = self._qdot_u_map self._km = km fraux = km._form_fr(loads) frstaraux = km._form_frstar(bodies) self._aux_eq = fraux + frstaraux self._fr = fr.col_join(fraux) self._frstar = frstar.col_join(frstaraux) return (self._fr, self._frstar) def rhs(self, inv_method=None): """Returns the system's equations of motion in first order form. The output is the right hand side of:: x' = |q'| =: f(q, u, r, p, t) |u'| The right hand side is what is needed by most numerical ODE integrators. Parameters ========== inv_method : str The specific sympy inverse matrix calculation method to use. For a list of valid methods, see :meth:`~sympy.matrices.matrices.MatrixBase.inv` """ rhs = zeros(len(self.q) + len(self.u), c=1) kdes = self.kindiffdict() for i, q_i in enumerate(self.q): rhs[i] = kdes[q_i.diff()] if inv_method is None: rhs[len(self.q):, 0] = self.mass_matrix.LUsolve(self.forcing) else: rhs[len(self.q):, 0] = (self.mass_matrix.inv(inv_method, try_block_diag=True) * self.forcing) return rhs def kindiffdict(self): """Returns a dictionary mapping q' to u.""" if not self._qdot_u_map: raise AttributeError('Create an instance of KanesMethod with ' 'kinematic differential equations to use this method.') return self._qdot_u_map @property def auxiliary_eqs(self): """A matrix containing the auxiliary equations.""" if not self._fr or not self._frstar: raise ValueError('Need to compute Fr, Fr* first.') if not self._uaux: raise ValueError('No auxiliary speeds have been declared.') return self._aux_eq @property def mass_matrix(self): """The mass matrix of the system.""" if not self._fr or not self._frstar: raise ValueError('Need to compute Fr, Fr* first.') return Matrix([self._k_d, self._k_dnh]) @property def mass_matrix_full(self): """The mass matrix of the system, augmented by the kinematic differential equations.""" if not self._fr or not self._frstar: raise ValueError('Need to compute Fr, Fr* first.') o = len(self.u) n = len(self.q) return ((self._k_kqdot).row_join(zeros(n, o))).col_join((zeros(o, n)).row_join(self.mass_matrix)) @property def forcing(self): """The forcing vector of the system.""" if not self._fr or not self._frstar: raise ValueError('Need to compute Fr, Fr* first.') return -Matrix([self._f_d, self._f_dnh]) @property def forcing_full(self): """The forcing vector of the system, augmented by the kinematic differential equations.""" if not self._fr or not self._frstar: raise ValueError('Need to compute Fr, Fr* first.') f1 = self._k_ku * Matrix(self.u) + self._f_k return -Matrix([f1, self._f_d, self._f_dnh]) @property def q(self): return self._q @property def u(self): return self._u @property def bodylist(self): return self._bodylist @property def forcelist(self): return self._forcelist