(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, q4 = symbols('q0 q1 q2 q3 q4')
        >>> 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])
Defines the orientation of this frame relative to a parent frame.
Defines the orientation of this frame relative to a parent frame .
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, q4 = symbols('q0 q1 q2 q3 q4')
        >>> 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()
        for frame in frames:
            if frame in self._dcm_dict:
                del frame._dcm_dict[self]
            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 = dynamicsymbols('u1, u2, u3')
                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 orient ( self , parent , rot_type , amounts , rot_order = '' ) : 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 ( ) for frame in frames : if frame in self . _dcm_dict : del frame . _dcm_dict [ self ] 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 = dynamicsymbols ( 'u1, u2, u3' ) 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 = { }