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 time_derivative(expr, frame, order=1): """ Calculate the time derivative of a vector/scalar field function or dyadic expression in given frame. References ========== https://en.wikipedia.org/wiki/Rotating_reference_frame#Time_derivatives_in_the_two_frames Parameters ========== expr : Vector/Dyadic/sympifyable The expression whose time derivative is to be calculated frame : ReferenceFrame The reference frame to calculate the time derivative in order : integer The order of the derivative to be calculated Examples ======== >>> from sympy.physics.vector import ReferenceFrame, dynamicsymbols >>> from sympy import Symbol >>> q1 = Symbol('q1') >>> u1 = dynamicsymbols('u1') >>> N = ReferenceFrame('N') >>> A = N.orientnew('A', 'Axis', [q1, N.x]) >>> v = u1 * N.x >>> A.set_ang_vel(N, 10*A.x) >>> from sympy.physics.vector import time_derivative >>> time_derivative(v, N) u1'*N.x >>> time_derivative(u1*A[0], N) N_x*Derivative(u1(t), t) >>> B = N.orientnew('B', 'Axis', [u1, N.z]) >>> from sympy.physics.vector import outer >>> d = outer(N.x, N.x) >>> time_derivative(d, B) - u1'*(N.y|N.x) - u1'*(N.x|N.y) """ t = dynamicsymbols._t _check_frame(frame) if order == 0: return expr if order % 1 != 0 or order < 0: raise ValueError("Unsupported value of order entered") if isinstance(expr, Vector): outlist = [] for i, v in enumerate(expr.args): if v[1] == frame: outlist += [(express(v[0], frame, variables=True).diff(t), frame)] else: outlist += (time_derivative(Vector([v]), v[1]) + \ (v[1].ang_vel_in(frame) ^ Vector([v]))).args outvec = Vector(outlist) return time_derivative(outvec, frame, order - 1) if isinstance(expr, Dyadic): ol = Dyadic(0) for i, v in enumerate(expr.args): ol += (v[0].diff(t) * (v[1] | v[2])) ol += (v[0] * (time_derivative(v[1], frame) | v[2])) ol += (v[0] * (v[1] | time_derivative(v[2], frame))) return time_derivative(ol, frame, order - 1) else: return diff(express(expr, frame, variables=True), t, order)
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 _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 _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 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 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', '') rot_order = str( rot_order).upper() # Now we need to make sure XYZ = 123 rot_type = rot_type.upper() rot_order = [i.replace('X', '1') for i in rot_order] rot_order = [i.replace('Y', '2') for i in rot_order] rot_order = [i.replace('Z', '3') for i in rot_order] rot_order = ''.join(rot_order) 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 to_linearizer(self, q_ind=None, qd_ind=None, q_dep=None, qd_dep=None): """Returns an instance of the Linearizer class, initiated from the data in the LagrangesMethod class. This may be more desirable than using the linearize class method, as the Linearizer object will allow more efficient recalculation (i.e. about varying operating points). Parameters ========== q_ind, qd_ind : array_like, optional The independent generalized coordinates and speeds. q_dep, qd_dep : array_like, optional The dependent generalized coordinates and speeds. """ # Compose vectors t = dynamicsymbols._t q = self.q u = self._qdots ud = u.diff(t) # Get vector of lagrange multipliers lams = self.lam_vec mat_build = lambda x: Matrix(x) if x else Matrix() q_i = mat_build(q_ind) q_d = mat_build(q_dep) u_i = mat_build(qd_ind) u_d = mat_build(qd_dep) # Compose general form equations f_c = self._hol_coneqs f_v = self.coneqs f_a = f_v.diff(t) f_0 = u f_1 = -u f_2 = self._term1 f_3 = -(self._term2 + self._term4) f_4 = -self._term3 # Check that there are an appropriate number of independent and # dependent coordinates if len(q_d) != len(f_c) or len(u_d) != len(f_v): raise ValueError( ("Must supply {:} dependent coordinates, and " + "{:} dependent speeds").format(len(f_c), len(f_v))) if set(Matrix([q_i, q_d])) != set(q): raise ValueError("Must partition q into q_ind and q_dep, with " + "no extra or missing symbols.") if set(Matrix([u_i, u_d])) != set(u): raise ValueError("Must partition qd into qd_ind and qd_dep, " + "with no extra or missing symbols.") # Find all other dynamic symbols, forming the forcing vector r. # Sort r to make it canonical. insyms = set(Matrix([q, u, ud, lams])) r = list(find_dynamicsymbols(f_3, insyms)) r.sort(key=default_sort_key) # Check for any derivatives of variables in r that are also found in r. for i in r: if diff(i, dynamicsymbols._t) in r: raise ValueError('Cannot have derivatives of specified \ quantities when linearizing forcing terms.') return Linearizer(f_0, f_1, f_2, f_3, f_4, f_c, f_v, f_a, q, u, q_i, q_d, u_i, u_d, r, lams)
def to_linearizer(self, q_ind=None, qd_ind=None, q_dep=None, qd_dep=None): """Returns an instance of the Linearizer class, initiated from the data in the LagrangesMethod class. This may be more desirable than using the linearize class method, as the Linearizer object will allow more efficient recalculation (i.e. about varying operating points). Parameters ========== q_ind, qd_ind : array_like, optional The independent generalized coordinates and speeds. q_dep, qd_dep : array_like, optional The dependent generalized coordinates and speeds. """ # Compose vectors t = dynamicsymbols._t q = self.q u = self._qdots ud = u.diff(t) # Get vector of lagrange multipliers lams = self.lam_vec mat_build = lambda x: Matrix(x) if x else Matrix() q_i = mat_build(q_ind) q_d = mat_build(q_dep) u_i = mat_build(qd_ind) u_d = mat_build(qd_dep) # Compose general form equations f_c = self._hol_coneqs f_v = self.coneqs f_a = f_v.diff(t) f_0 = u f_1 = -u f_2 = self._term1 f_3 = -(self._term2 + self._term4) f_4 = -self._term3 # Check that there are an appropriate number of independent and # dependent coordinates if len(q_d) != len(f_c) or len(u_d) != len(f_v): raise ValueError(("Must supply {:} dependent coordinates, and " + "{:} dependent speeds").format(len(f_c), len(f_v))) if set(Matrix([q_i, q_d])) != set(q): raise ValueError("Must partition q into q_ind and q_dep, with " + "no extra or missing symbols.") if set(Matrix([u_i, u_d])) != set(u): raise ValueError("Must partition qd into qd_ind and qd_dep, " + "with no extra or missing symbols.") # Find all other dynamic symbols, forming the forcing vector r. # Sort r to make it canonical. insyms = set(Matrix([q, u, ud, lams])) r = list(find_dynamicsymbols(f_3, insyms)) r.sort(key=default_sort_key) # Check for any derivatives of variables in r that are also found in r. for i in r: if diff(i, dynamicsymbols._t) in r: raise ValueError('Cannot have derivatives of specified \ quantities when linearizing forcing terms.') return Linearizer(f_0, f_1, f_2, f_3, f_4, f_c, f_v, f_a, q, u, q_i, q_d, u_i, u_d, r, lams)
def 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 = {}
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 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 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', '') rot_order = str(rot_order).upper() # Now we need to make sure XYZ = 123 rot_type = rot_type.upper() rot_order = [i.replace('X', '1') for i in rot_order] rot_order = [i.replace('Y', '2') for i in rot_order] rot_order = [i.replace('Z', '3') for i in rot_order] rot_order = ''.join(rot_order) 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.lower() 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 q1, q2, q3 = coords q1d, q2d, q3d = [diff(i, dynamicsymbols._t) for i in coords] w1, w2, w3 = speeds s1, s2, s3 = [sin(q1), sin(q2), sin(q3)] c1, c2, c3 = [cos(q1), cos(q2), cos(q3)] if rot_type.lower() == '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.lower() == '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.lower() == '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 time_derivative(expr, frame, order=1): """ Calculate the time derivative of a vector/scalar field function or dyadic expression in given frame. References ========== http://en.wikipedia.org/wiki/Rotating_reference_frame#Time_derivatives_in_the_two_frames Parameters ========== expr : Vector/Dyadic/sympifyable The expression whose time derivative is to be calculated frame : ReferenceFrame The reference frame to calculate the time derivative in order : integer The order of the derivative to be calculated Examples ======== >>> from sympy.physics.vector import ReferenceFrame, dynamicsymbols >>> from sympy import Symbol >>> q1 = Symbol('q1') >>> u1 = dynamicsymbols('u1') >>> N = ReferenceFrame('N') >>> A = N.orientnew('A', 'Axis', [q1, N.x]) >>> v = u1 * N.x >>> A.set_ang_vel(N, 10*A.x) >>> from sympy.physics.vector import time_derivative >>> time_derivative(v, N) u1'*N.x >>> time_derivative(u1*A[0], N) N_x*Derivative(u1(t), t) >>> B = N.orientnew('B', 'Axis', [u1, N.z]) >>> from sympy.physics.vector import outer >>> d = outer(N.x, N.x) >>> time_derivative(d, B) - u1'*(N.y|N.x) - u1'*(N.x|N.y) """ t = dynamicsymbols._t _check_frame(frame) if order == 0: return expr if order % 1 != 0 or order < 0: raise ValueError("Unsupported value of order entered") if isinstance(expr, Vector): outlist = [] for i, v in enumerate(expr.args): if v[1] == frame: outlist += [(express(v[0], frame, variables=True).diff(t), frame)] else: outlist += (time_derivative(Vector([v]), v[1]) + \ (v[1].ang_vel_in(frame) ^ Vector([v]))).args outvec = Vector(outlist) return time_derivative(outvec, frame, order - 1) if isinstance(expr, Dyadic): ol = Dyadic(0) for i, v in enumerate(expr.args): ol += (v[0].diff(t) * (v[1] | v[2])) ol += (v[0] * (time_derivative(v[1], frame) | v[2])) ol += (v[0] * (v[1] | time_derivative(v[2], frame))) return time_derivative(ol, frame, order - 1) else: return diff(express(expr, frame, variables=True), t, order)
def orient_quaternion(self, parent, numbers): """Sets the orientation of this reference frame relative to a parent reference frame via an orientation quaternion. An orientation quaternion is defined as a finite rotation a unit vector, ``(lambda_x, lambda_y, lambda_z)``, by an angle ``theta``. The orientation quaternion 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)`` See `Quaternions and Spatial Rotation <https://en.wikipedia.org/wiki/Quaternions_and_spatial_rotation>`_ on Wikipedia for more information. Parameters ========== parent : ReferenceFrame Reference frame that this reference frame will be rotated relative to. numbers : 4-tuple of sympifiable The four quaternion scalar numbers as defined above: ``q0``, ``q1``, ``q2``, ``q3``. 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') Set the orientation: >>> B.orient_quaternion(N, (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) numbers = list(numbers) for i, v in enumerate(numbers): if not isinstance(v, Vector): numbers[i] = sympify(v) parent_orient_quaternion = [] if not (isinstance(numbers, (list, tuple)) & (len(numbers) == 4)): raise TypeError('Amounts are a list or tuple of length 4') q0, q1, q2, q3 = numbers parent_orient_quaternion = ( 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]])) self._dcm(parent, parent_orient_quaternion) t = dynamicsymbols._t q0, q1, q2, q3 = numbers 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)]) self._ang_vel_dict.update({parent: wvec}) parent._ang_vel_dict.update({self: -wvec}) self._var_dict = {}
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))