def __xor__(self, other): """For a cross product in the form: Dyadic x Vector. Parameters ========== other : Vector The Vector that we are crossing this Dyadic with Examples ======== >>> from sympy.physics.vector import ReferenceFrame, outer, cross >>> N = ReferenceFrame('N') >>> d = outer(N.x, N.x) >>> cross(d, N.y) (N.x|N.z) """ from sympy.physics.vector.vector import _check_vector other = _check_vector(other) ol = Dyadic(0) for i, v in enumerate(self.args): ol += v[0] * (v[1] | (v[2] ^ other)) return ol
def set_pos(self, otherpoint, value): """Used to set the position of this point w.r.t. another point. Parameters ========== value : Vector The vector which defines the location of this point point : Point The other point which this point's location is defined relative to Examples ======== >>> from sympy.physics.vector import Point, ReferenceFrame >>> N = ReferenceFrame('N') >>> p1 = Point('p1') >>> p2 = Point('p2') >>> p1.set_pos(p2, 10 * N.x) >>> p1.pos_from(p2) 10*N.x """ if value == 0: value = Vector(0) value = _check_vector(value) self._check_point(otherpoint) self._pos_dict.update({otherpoint: value}) otherpoint._pos_dict.update({self: -value})
def set_vel(self, frame, value): """Sets the velocity Vector of this Point in a ReferenceFrame. Parameters ========== value : Vector The vector value of this point's velocity in the frame frame : ReferenceFrame The frame in which this point's velocity is defined Examples ======== >>> from sympy.physics.vector import Point, ReferenceFrame >>> N = ReferenceFrame('N') >>> p1 = Point('p1') >>> p1.set_vel(N, 10 * N.x) >>> p1.vel(N) 10*N.x """ if value == 0: value = Vector(0) value = _check_vector(value) _check_frame(frame) self._vel_dict.update({frame: value})
def set_acc(self, frame, value): """Used to set the acceleration of this Point in a ReferenceFrame. Parameters ========== value : Vector The vector value of this point's acceleration in the frame frame : ReferenceFrame The frame in which this point's acceleration is defined Examples ======== >>> from sympy.physics.vector import Point, ReferenceFrame >>> N = ReferenceFrame('N') >>> p1 = Point('p1') >>> p1.set_acc(N, 10 * N.x) >>> p1.acc(N) 10*N.x """ if value == 0: value = Vector(0) value = _check_vector(value) _check_frame(frame) self._acc_dict.update({frame: value})
def locatenew(self, name, value): """Creates a new point with a position defined from this point. Parameters ========== name : str The name for the new point value : Vector The position of the new point relative to this point Examples ======== >>> from sympy.physics.vector import ReferenceFrame, Point >>> N = ReferenceFrame('N') >>> P1 = Point('P1') >>> P2 = P1.locatenew('P2', 10 * N.x) """ if not isinstance(name, str): raise TypeError('Must supply a valid name') if value == 0: value = Vector(0) value = _check_vector(value) p = Point(name) p.set_pos(self, value) self.set_pos(p, -value) return p
def __and__(self, other): """The inner product operator for a Dyadic and a Dyadic or Vector. Parameters ========== other : Dyadic or Vector The other Dyadic or Vector to take the inner product with Examples ======== >>> from sympy.physics.vector import ReferenceFrame, outer >>> N = ReferenceFrame('N') >>> D1 = outer(N.x, N.y) >>> D2 = outer(N.y, N.y) >>> D1.dot(D2) (N.x|N.y) >>> D1.dot(N.y) N.x """ from sympy.physics.vector.vector import Vector, _check_vector if isinstance(other, Dyadic): other = _check_dyadic(other) ol = Dyadic(0) for i, v in enumerate(self.args): for i2, v2 in enumerate(other.args): ol += v[0] * v2[0] * (v[2] & v2[1]) * (v[1] | v2[2]) else: other = _check_vector(other) ol = Vector(0) for i, v in enumerate(self.args): ol += v[0] * v[1] * (v[2] & other) return ol
def __rand__(self, other): """The inner product operator for a Vector or Dyadic, and a Dyadic This is for: Vector dot Dyadic Parameters ========== other : Vector The vector we are dotting with Examples ======== >>> from sympy.physics.vector import ReferenceFrame, dot, outer >>> N = ReferenceFrame('N') >>> d = outer(N.x, N.x) >>> dot(N.x, d) N.x """ from sympy.physics.vector.vector import Vector, _check_vector other = _check_vector(other) ol = Vector(0) for i, v in enumerate(self.args): ol += v[0] * v[2] * (v[1] & other) return ol
def set_ang_vel(self, otherframe, value): """Define the angular velocity vector in a ReferenceFrame. Defines the angular velocity of this ReferenceFrame, in another. Angular velocity can be defined with respect to multiple different ReferenceFrames. Care must be taken to not create loops which are inconsistent. Parameters ========== otherframe : ReferenceFrame A ReferenceFrame to define the angular velocity in value : Vector The Vector representing angular velocity Examples ======== >>> from sympy.physics.vector import ReferenceFrame, Vector >>> N = ReferenceFrame('N') >>> A = ReferenceFrame('A') >>> V = 10 * N.x >>> A.set_ang_vel(N, V) >>> A.ang_vel_in(N) 10*N.x """ if value == 0: value = Vector(0) value = _check_vector(value) _check_frame(otherframe) self._ang_vel_dict.update({otherframe: value}) otherframe._ang_vel_dict.update({self: -value})
def curl(vect, frame): """ Returns the curl of a vector field computed wrt the coordinate symbols of the given frame. Parameters ========== vect : Vector The vector operand frame : ReferenceFrame The reference frame to calculate the curl in Examples ======== >>> from sympy.physics.vector import ReferenceFrame >>> from sympy.physics.vector import curl >>> R = ReferenceFrame('R') >>> v1 = R[1]*R[2]*R.x + R[0]*R[2]*R.y + R[0]*R[1]*R.z >>> curl(v1, R) 0 >>> v2 = R[0]*R[1]*R[2]*R.x >>> curl(v2, R) R_x*R_y*R.y - R_x*R_z*R.z """ _check_vector(vect) if vect == 0: return Vector(0) vect = express(vect, frame, variables=True) #A mechanical approach to avoid looping overheads vectx = vect.dot(frame.x) vecty = vect.dot(frame.y) vectz = vect.dot(frame.z) outvec = Vector(0) outvec += (diff(vectz, frame[1]) - diff(vecty, frame[2])) * frame.x outvec += (diff(vectx, frame[2]) - diff(vectz, frame[0])) * frame.y outvec += (diff(vecty, frame[0]) - diff(vectx, frame[1])) * frame.z return outvec
def divergence(vect, frame): """ Returns the divergence of a vector field computed wrt the coordinate symbols of the given frame. Parameters ========== vect : Vector The vector operand frame : ReferenceFrame The reference frame to calculate the divergence in Examples ======== >>> from sympy.physics.vector import ReferenceFrame >>> from sympy.physics.vector import divergence >>> R = ReferenceFrame('R') >>> v1 = R[0]*R[1]*R[2] * (R.x+R.y+R.z) >>> divergence(v1, R) R_x*R_y + R_x*R_z + R_y*R_z >>> v2 = 2*R[1]*R[2]*R.y >>> divergence(v2, R) 2*R_z """ _check_vector(vect) if vect == 0: return S(0) vect = express(vect, frame, variables=True) vectx = vect.dot(frame.x) vecty = vect.dot(frame.y) vectz = vect.dot(frame.z) out = S(0) out += diff(vectx, frame[0]) out += diff(vecty, frame[1]) out += diff(vectz, frame[2]) return out
def divergence(vect, frame): """ Returns the divergence of a vector field computed wrt the coordinate symbols of the given frame. Parameters ========== vect : Vector The vector operand frame : ReferenceFrame The reference frame to calculate the divergence in Examples ======== >>> from sympy.physics.vector import ReferenceFrame >>> from sympy.physics.vector import divergence >>> R = ReferenceFrame('R') >>> v1 = R[0]*R[1]*R[2] * (R.x+R.y+R.z) >>> divergence(v1, R) R_x*R_y + R_x*R_z + R_y*R_z >>> v2 = 2*R[1]*R[2]*R.y >>> divergence(v2, R) 2*R_z """ _check_vector(vect) if vect == 0: return S.Zero vect = express(vect, frame, variables=True) vectx = vect.dot(frame.x) vecty = vect.dot(frame.y) vectz = vect.dot(frame.z) out = S.Zero out += diff(vectx, frame[0]) out += diff(vecty, frame[1]) out += diff(vectz, frame[2]) return out
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', and 'Axis'. See examples for correct usage. amounts : list OR value The quantities that the orientation matrix will be defined by. rot_order : str If applicable, the order of a series of rotations. Examples ======== >>> from sympy.physics.vector import ReferenceFrame, Vector >>> from sympy import symbols >>> 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]) Last 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]) """ from sympy.physics.vector.functions import dynamicsymbols _check_frame(parent) 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])) 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() 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 get_motion_params(frame, **kwargs): """ Returns the three motion parameters - (acceleration, velocity, and position) as vectorial functions of time in the given frame. If a higher order differential function is provided, the lower order functions are used as boundary conditions. For example, given the acceleration, the velocity and position parameters are taken as boundary conditions. The values of time at which the boundary conditions are specified are taken from timevalue1(for position boundary condition) and timevalue2(for velocity boundary condition). If any of the boundary conditions are not provided, they are taken to be zero by default (zero vectors, in case of vectorial inputs). If the boundary conditions are also functions of time, they are converted to constants by substituting the time values in the dynamicsymbols._t time Symbol. This function can also be used for calculating rotational motion parameters. Have a look at the Parameters and Examples for more clarity. Parameters ========== frame : ReferenceFrame The frame to express the motion parameters in acceleration : Vector Acceleration of the object/frame as a function of time velocity : Vector Velocity as function of time or as boundary condition of velocity at time = timevalue1 position : Vector Velocity as function of time or as boundary condition of velocity at time = timevalue1 timevalue1 : sympyfiable Value of time for position boundary condition timevalue2 : sympyfiable Value of time for velocity boundary condition Examples ======== >>> from sympy.physics.vector import ReferenceFrame, get_motion_params, dynamicsymbols >>> from sympy import symbols >>> R = ReferenceFrame('R') >>> v1, v2, v3 = dynamicsymbols('v1 v2 v3') >>> v = v1*R.x + v2*R.y + v3*R.z >>> get_motion_params(R, position = v) (v1''*R.x + v2''*R.y + v3''*R.z, v1'*R.x + v2'*R.y + v3'*R.z, v1*R.x + v2*R.y + v3*R.z) >>> a, b, c = symbols('a b c') >>> v = a*R.x + b*R.y + c*R.z >>> get_motion_params(R, velocity = v) (0, a*R.x + b*R.y + c*R.z, a*t*R.x + b*t*R.y + c*t*R.z) >>> parameters = get_motion_params(R, acceleration = v) >>> parameters[1] a*t*R.x + b*t*R.y + c*t*R.z >>> parameters[2] a*t**2/2*R.x + b*t**2/2*R.y + c*t**2/2*R.z """ ##Helper functions def _process_vector_differential(vectdiff, condition, \ variable, ordinate, frame): """ Helper function for get_motion methods. Finds derivative of vectdiff wrt variable, and its integral using the specified boundary condition at value of variable = ordinate. Returns a tuple of - (derivative, function and integral) wrt vectdiff """ #Make sure boundary condition is independent of 'variable' if condition != 0: condition = express(condition, frame, variables=True) #Special case of vectdiff == 0 if vectdiff == Vector(0): return (0, 0, condition) #Express vectdiff completely in condition's frame to give vectdiff1 vectdiff1 = express(vectdiff, frame) #Find derivative of vectdiff vectdiff2 = time_derivative(vectdiff, frame) #Integrate and use boundary condition vectdiff0 = Vector(0) lims = (variable, ordinate, variable) for dim in frame: function1 = vectdiff1.dot(dim) abscissa = dim.dot(condition).subs({variable: ordinate}) # Indefinite integral of 'function1' wrt 'variable', using # the given initial condition (ordinate, abscissa). vectdiff0 += (integrate(function1, lims) + abscissa) * dim #Return tuple return (vectdiff2, vectdiff, vectdiff0) ##Function body _check_frame(frame) #Decide mode of operation based on user's input if 'acceleration' in kwargs: mode = 2 elif 'velocity' in kwargs: mode = 1 else: mode = 0 #All the possible parameters in kwargs #Not all are required for every case #If not specified, set to default values(may or may not be used in #calculations) conditions = [ 'acceleration', 'velocity', 'position', 'timevalue', 'timevalue1', 'timevalue2' ] for i, x in enumerate(conditions): if x not in kwargs: if i < 3: kwargs[x] = Vector(0) else: kwargs[x] = S(0) elif i < 3: _check_vector(kwargs[x]) else: kwargs[x] = sympify(kwargs[x]) if mode == 2: vel = _process_vector_differential(kwargs['acceleration'], kwargs['velocity'], dynamicsymbols._t, kwargs['timevalue2'], frame)[2] pos = _process_vector_differential(vel, kwargs['position'], dynamicsymbols._t, kwargs['timevalue1'], frame)[2] return (kwargs['acceleration'], vel, pos) elif mode == 1: return _process_vector_differential(kwargs['velocity'], kwargs['position'], dynamicsymbols._t, kwargs['timevalue1'], frame) else: vel = time_derivative(kwargs['position'], frame) acc = time_derivative(vel, frame) return (acc, vel, kwargs['position'])
def orient_axis(self, parent, axis, angle): """Sets the orientation of this reference frame with respect to a parent reference frame by rotating through an angle about an axis fixed in the parent reference frame. Parameters ========== parent : ReferenceFrame Reference frame that this reference frame will be rotated relative to. axis : Vector Vector fixed in the parent frame about about which this frame is rotated. It need not be a unit vector and the rotation follows the right hand rule. angle : sympifiable Angle in radians by which it the frame is to be rotated. Examples ======== Setup variables for the examples: >>> from sympy import symbols >>> from sympy.physics.vector import ReferenceFrame >>> q1 = symbols('q1') >>> N = ReferenceFrame('N') >>> B = ReferenceFrame('B') >>> B.orient_axis(N, N.x, q1) The ``orient_axis()`` 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)]]) >>> N.dcm(B) Matrix([ [1, 0, 0], [0, cos(q1), -sin(q1)], [0, sin(q1), cos(q1)]]) The following two lines show that the sense of the rotation can be defined by negating the vector direction or the angle. Both lines produce the same result. >>> B.orient_axis(N, -N.x, q1) >>> B.orient_axis(N, N.x, -q1) """ from sympy.physics.vector.functions import dynamicsymbols _check_frame(parent) amount = sympify(angle) theta = amount axis = _check_vector(axis) parent_orient_axis = [] if not axis.dt(parent) == 0: raise ValueError('Axis cannot be time-varying.') unit_axis = axis.express(parent).normalize() unit_col = unit_axis.args[0][0] parent_orient_axis = ( (eye(3) - unit_col * unit_col.T) * cos(theta) + Matrix([[0, -unit_col[2], unit_col[1]], [unit_col[2], 0, -unit_col[0]], [-unit_col[1], unit_col[0], 0]]) * sin(theta) + unit_col * unit_col.T) self._dcm(parent, parent_orient_axis) thetad = (amount).diff(dynamicsymbols._t) wvec = thetad*axis.express(parent).normalize() self._ang_vel_dict.update({parent: wvec}) parent._ang_vel_dict.update({self: -wvec}) self._var_dict = {}
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 get_motion_params(frame, **kwargs): """ Returns the three motion parameters - (acceleration, velocity, and position) as vectorial functions of time in the given frame. If a higher order differential function is provided, the lower order functions are used as boundary conditions. For example, given the acceleration, the velocity and position parameters are taken as boundary conditions. The values of time at which the boundary conditions are specified are taken from timevalue1(for position boundary condition) and timevalue2(for velocity boundary condition). If any of the boundary conditions are not provided, they are taken to be zero by default (zero vectors, in case of vectorial inputs). If the boundary conditions are also functions of time, they are converted to constants by substituting the time values in the dynamicsymbols._t time Symbol. This function can also be used for calculating rotational motion parameters. Have a look at the Parameters and Examples for more clarity. Parameters ========== frame : ReferenceFrame The frame to express the motion parameters in acceleration : Vector Acceleration of the object/frame as a function of time velocity : Vector Velocity as function of time or as boundary condition of velocity at time = timevalue1 position : Vector Velocity as function of time or as boundary condition of velocity at time = timevalue1 timevalue1 : sympyfiable Value of time for position boundary condition timevalue2 : sympyfiable Value of time for velocity boundary condition Examples ======== >>> from sympy.physics.vector import ReferenceFrame, get_motion_params, dynamicsymbols >>> from sympy import symbols >>> R = ReferenceFrame('R') >>> v1, v2, v3 = dynamicsymbols('v1 v2 v3') >>> v = v1*R.x + v2*R.y + v3*R.z >>> get_motion_params(R, position = v) (v1''*R.x + v2''*R.y + v3''*R.z, v1'*R.x + v2'*R.y + v3'*R.z, v1*R.x + v2*R.y + v3*R.z) >>> a, b, c = symbols('a b c') >>> v = a*R.x + b*R.y + c*R.z >>> get_motion_params(R, velocity = v) (0, a*R.x + b*R.y + c*R.z, a*t*R.x + b*t*R.y + c*t*R.z) >>> parameters = get_motion_params(R, acceleration = v) >>> parameters[1] a*t*R.x + b*t*R.y + c*t*R.z >>> parameters[2] a*t**2/2*R.x + b*t**2/2*R.y + c*t**2/2*R.z """ ##Helper functions def _process_vector_differential(vectdiff, condition, \ variable, ordinate, frame): """ Helper function for get_motion methods. Finds derivative of vectdiff wrt variable, and its integral using the specified boundary condition at value of variable = ordinate. Returns a tuple of - (derivative, function and integral) wrt vectdiff """ #Make sure boundary condition is independent of 'variable' if condition != 0: condition = express(condition, frame, variables=True) #Special case of vectdiff == 0 if vectdiff == Vector(0): return (0, 0, condition) #Express vectdiff completely in condition's frame to give vectdiff1 vectdiff1 = express(vectdiff, frame) #Find derivative of vectdiff vectdiff2 = time_derivative(vectdiff, frame) #Integrate and use boundary condition vectdiff0 = Vector(0) lims = (variable, ordinate, variable) for dim in frame: function1 = vectdiff1.dot(dim) abscissa = dim.dot(condition).subs({variable : ordinate}) # Indefinite integral of 'function1' wrt 'variable', using # the given initial condition (ordinate, abscissa). vectdiff0 += (integrate(function1, lims) + abscissa) * dim #Return tuple return (vectdiff2, vectdiff, vectdiff0) ##Function body _check_frame(frame) #Decide mode of operation based on user's input if 'acceleration' in kwargs: mode = 2 elif 'velocity' in kwargs: mode = 1 else: mode = 0 #All the possible parameters in kwargs #Not all are required for every case #If not specified, set to default values(may or may not be used in #calculations) conditions = ['acceleration', 'velocity', 'position', 'timevalue', 'timevalue1', 'timevalue2'] for i, x in enumerate(conditions): if x not in kwargs: if i < 3: kwargs[x] = Vector(0) else: kwargs[x] = S(0) elif i < 3: _check_vector(kwargs[x]) else: kwargs[x] = sympify(kwargs[x]) if mode == 2: vel = _process_vector_differential(kwargs['acceleration'], kwargs['velocity'], dynamicsymbols._t, kwargs['timevalue2'], frame)[2] pos = _process_vector_differential(vel, kwargs['position'], dynamicsymbols._t, kwargs['timevalue1'], frame)[2] return (kwargs['acceleration'], vel, pos) elif mode == 1: return _process_vector_differential(kwargs['velocity'], kwargs['position'], dynamicsymbols._t, kwargs['timevalue1'], frame) else: vel = time_derivative(kwargs['position'], frame) acc = time_derivative(vel, frame) return (acc, vel, kwargs['position'])
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 = {}