(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']
Gives equations relating the qdot's to u's for a rotation type.
Gives equations relating the qdot s to u s for a rotation type .
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 kinematic_equations ( speeds , coords , rot_type , rot_order = '' ) : # 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' )