[SOLVED] Calculating the new position of a parented controller after rotating the parent in Maya using Python

I'm creating code to create a motion path of a controller based on it's keyframed positions in Maya. I've run into a problem when trying to use this code to create a motion path of a parented controller. If I rotate and translate the parent the generated motion path does not reflect the actual path of motion. Instead it creates the motion path as if it was not affected by the parent. I've looked around and found information for applying rotation using matrix transformations to the current position but it seems to be rotating it way too much. I've included the function for creating the motion path, it's a little long but the part that isn't working is within the else statement when dealing with the upper torso controller.

Old Code

#
# This function creates an animation curve within the scene that follows the path of motion
# of the selected controller. It requires keyframe information in order to genereate the curve
# and uses the range of frames given by the user.
#
def createAnimCurve( bodyField, startField, endField, firstColor ):
    # Takes the value of the text field to select the controller
    obj = cmds.textField(bodyField, query=True, text=True)
    print obj
    # Takes in the string input of the paramter values and turns them into integer values
    startFrame = cmds.intField(startField, query=True, value=True)
    print startFrame
    endFrame = cmds.intField(endField, query=True, value=True)
    print endFrame
    color = cmds.colorIndexSliderGrp( firstColor, query=True, value=True ) - 1
    print color

    if obj == "":
        cmds.warning( "WARNING: Need to Select Body Part from Diagram" )
        return
    if cmds.objExists(obj[:-3]+'Path'):
        # Creates a warning pop up that double checks if the user wants to remove the curve
        delRes = cmds.confirmDialog( title='Delete Path Warning', message='Recreation will delete current path. Are you sure?', button=['Yes','No'], defaultButton='Yes', cancelButton='No', dismissString='No' )
        # If yes then the curve is deleted
        if delRes == 'Yes':
            #cmds.delete(obj[:-3]+'ScalePath')  
            #cmds.delete(obj[:-3]+'ScalePath_LOC')  
            cmds.delete(obj[:-3]+'Path')     
            cmds.delete(obj[:-3]+'Path_LOC')
        else:
            return
    # Sorts through the list of keyframes of the selected obj from the selected time line
    global keyframes
    keyframes = sorted(list(set(cmds.keyframe(obj, q=True, time=(startFrame,endFrame), timeChange=True))))
    # Creates the arrays for the required point positions
    points = []
    centerPoints = []
    centerRotates = []
    combinedPoints = []

    # Special cases for controllers that are named differently than their joints
    if obj == "L_foot_CTL" or obj == "R_foot_CTL":
        loc = obj[:-4] + "Ankle_LOC"
    elif obj == "M_upTorso_CTL":
        loc = "M_spineTip_LOC"
    else:    
        loc = obj[:-3] + "LOC"
    # Grabs the original world space position to calculate the approraite motion points
    locPos = cmds.getAttr(loc+".translate")
    centerLocPos = cmds.getAttr("M_centerMass_LOC.translate")

    #for step in range( startFrame, endFrame+2, int(curveCVstep)):
    for step in range(len(keyframes)):
        # Moves throughout the specified timeline to find point results
        cmds.currentTime( keyframes[step] )
        if obj != "M_upTorso_CTL":
            # Queries the position of the controller to draw the curve
            # Adds the position of the controller in world space to draw it relative to the control
            pos = cmds.xform( obj,q=True,ws=True,t=True )
            pos[0] = pos[0] + locPos[0][0]
            pos[1] = pos[1] + locPos[0][1] 
            pos[2] = pos[2] + locPos[0][2]
            # convert the tuple (vector) to a string
            points.append(pos)
            print pos
        else:
            spineLength = cmds.getAttr('spineCurveInfo.arcLength')

            # Queries the position of the controller to draw the curve
            # Adds the position of the controller in world space to draw it relative to the control
            # adds in the spine length to the y position to take into consideration the offset of the centerMass controller
            pos = cmds.xform( obj,q=True,ws=True,t=True )
            pos[0] = pos[0] + locPos[0][0]
            pos[1] = pos[1] + locPos[0][1]
            pos[2] = pos[2] + locPos[0][2]
            # convert the tuple (vector) to a string
            print "Printing out points"
            points.append(pos)
            print pos

            # Queries the position of the center of mass controller 
            centerPos = cmds.xform( "M_centerMass_CTL",q=1,os=1,t=1 )
            centerPos[0] = centerPos[0] #+ centerLocPos[0][0]
            centerPos[1] = centerPos[1] #+ centerLocPos[0][1]
            centerPos[2] = centerPos[2] #+ centerLocPos[0][2]
            # convert the tuple (vector) to a string
            print "Printing out center Points"
            centerPoints.append(centerPos)
            print centerPos

            # Combine the two point positions to find the relative position 
            combinedPos = []
            combinedPos1 = pos[0] + centerPos[0]
            combinedPos.append(combinedPos1)
            combinedPos2 = pos[1] + centerPos[1]
            combinedPos.append(combinedPos2)
            combinedPos3 = pos[2] + centerPos[2]
            combinedPos.append(combinedPos3)
            print "Printing out combined Points"
            print combinedPos

            # Queries the rotation of the center of mass controller
            #centerRot = cmds.xform( "M_centerMass_CTL",q=1,ws=1,ro=1 )
            #centerRotates.append(centerRot)
            #print "Printing out rotations"
            #print centerRot
            # applies rotation of the center of mass controller to the upper torso controller
            # rotation around the Z axis
            #tempX = combinedPos[0]*math.cos(math.radians(centerRot[2])) - combinedPos[1]*math.sin(math.radians(centerRot[2]))
            #tempY = combinedPos[0]*math.sin(math.radians(centerRot[2])) + combinedPos[1]*math.cos(math.radians(centerRot[2]))
            # rotation around the Y axis
            #tempX2 = tempX*math.cos(math.radians(centerRot[1])) + combinedPos[2]*math.sin(math.radians(centerRot[1]))
            #tempZ = combinedPos[2]*math.cos(math.radians(centerRot[1])) - tempX*math.sin(math.radians(centerRot[1]))
            # rotation around the X axis
            #tempY2 = tempY*math.cos(math.radians(centerRot[0])) - tempZ*math.sin(math.radians(centerRot[0]))
            #tempZ2 = tempY*math.sin(math.radians(centerRot[0])) + tempZ*math.cos(math.radians(centerRot[0]))

            #combinedPos[0] = tempX2
            #combinedPos[1] = tempY2
            #combinedPos[2] = tempZ2
            #print "Printing out rotated Points"
            combinedPoints.append(combinedPos)
            print combinedPos

    # if the obj is the upper torso controller we need to take into consideration the center of mass controller
    # Creates the motion curve with the required cvs
    if obj == "M_upTorso_CTL":
        cur = cmds.curve(d=2, ws=True, p=combinedPoints, n=obj[:-3]+'Path')
        cmds.setAttr(cur + '.overrideEnabled', 1)
        cmds.setAttr(cur + '.overrideColor', color)
        print cur
        cmds.move(points[0][0], points[0][1], points[0][2], cur+".scalePivot", cur+".rotatePivot", absolute=True)
    else:
        cur = cmds.curve(d=2, ws=True, p=points, n=obj[:-3]+'Path')
        cmds.setAttr(cur + '.overrideEnabled', 1)
        cmds.setAttr(cur + '.overrideColor', color) 
        print cur
        cmds.move(points[0][0], points[0][1], points[0][2], cur+".scalePivot", cur+".rotatePivot", absolute=True)
    # command that runs through each cv of the curve and returns their position within a list.
    cvs = cmds.getAttr( obj[:-3]+'Path.cv[*]' )
    print cvs

    global initCVS
    initCVS = cvs
    # Create a locator for the motion path that the controller will now follow
    locate = cmds.spaceLocator( n=obj[:-3]+"Path_LOC" )
    #for step in range( startFrame, endFrame+2, int(curveCVstep)):
    for step in range(len(keyframes)):
        # Moves throughout the specified timeline to find point results
        cmds.currentTime( keyframes[step] )
        # Moves the locator to match the position of the controller
        cmds.move( cvs[step][0], cvs[step][1], cvs[step][2], locate)
        # Keyframes the locator
        cmds.setKeyframe( locate )
    # Position obj at the location of locate.
    cmds.pointConstraint( locate, obj, n=obj[:-3]+"LOC1_PNT" )
    cmds.setAttr( loc+'.visibility', 0)
    # keys the weight of the point constraint to 0 before and after time frame (set to 1 during time frame)
    #Before startFrame
    cmds.currentTime( startFrame - 1 )
    cmds.setAttr(obj+'.blendPoint1', 0 )
    cmds.setKeyframe(obj+'.blendPoint1' )
    #After startframe
    cmds.currentTime( startFrame )
    cmds.setAttr(obj+'.blendPoint1', 1 )
    cmds.setKeyframe(obj+'.blendPoint1' )
    #Before endframe
    cmds.currentTime( endFrame )
    cmds.setAttr(obj+'.blendPoint1', 1 )
    cmds.setKeyframe(obj+'.blendPoint1' )
    #After endframe
    cmds.currentTime( endFrame + 1 )
    cmds.setAttr(obj+'.blendPoint1', 0 )
    cmds.setKeyframe(obj+'.blendPoint1' )
    cmds.select(obj)

The issue with the code was that I froze the transformations on my controllers setting the pivot to (0,0,0) in world space. The best way to fix this is to create a temporary locator and have it follow the controller. Use the positions of the temp locator to create the motion path of the controller. Once created delete the temp locator.

New Code

#
# This function creates an animation curve within the scene that follows the path of motion
# of the selected controller. It requires keyframe information in order to genereate the curve
# and uses the range of frames given by the user.
#
def createAnimCurve( bodyField, startField, endField, firstColor ):
    # Takes the value of the text field to select the controller
    obj = cmds.textField(bodyField, query=True, text=True)
    print obj
    # Takes in the string input of the paramter values and turns them into integer values
    startFrame = cmds.intField(startField, query=True, value=True)
    print startFrame
    endFrame = cmds.intField(endField, query=True, value=True)
    print endFrame
    color = cmds.colorIndexSliderGrp( firstColor, query=True, value=True ) - 1
    print color

    if obj == "":
        cmds.warning( "WARNING: Need to Select Body Part from Diagram" )
        return
    if cmds.objExists(obj[:-3]+'Path'):
        # Creates a warning pop up that double checks if the user wants to remove the curve
        delRes = cmds.confirmDialog( title='Delete Path Warning', message='Recreation will delete current path. Are you sure?', button=['Yes','No'], defaultButton='Yes', cancelButton='No', dismissString='No' )
        # If yes then the curve is deleted
        if delRes == 'Yes':
            cmds.delete(obj[:-3]+'Path')     
            cmds.delete(obj[:-3]+'Path_LOC')
        else:
            return
    # Sorts through the list of keyframes of the selected obj from the selected time line
    global keyframes
    keyframes = sorted(list(set(cmds.keyframe(obj, q=True, time=(startFrame,endFrame), timeChange=True))))
    # Creates the arrays for the required point positions
    points = []

    # Creates a temporary locator to find the world space values of the controller
    cmds.spaceLocator( n="tempLoc" )
    cmds.parentConstraint( obj, 'tempLoc', n='temp_PRT_CST' )

    #for step in range( startFrame, endFrame+2, int(curveCVstep)):
    for step in range(len(keyframes)):
        # Moves throughout the specified timeline to find point results
        cmds.currentTime( keyframes[step] )
        # Queries the position of the controller to draw the curve
        # Adds the position of the controller in world space to draw it relative to the control
        pos = cmds.xform( "tempLoc",q=True,ws=True,t=True )
        pos[0] = pos[0] 
        pos[1] = pos[1] 
        pos[2] = pos[2] 
        # convert the tuple (vector) to a string
        points.append(pos)
        print pos

    print "Creating the basic motion curve"
    cur = cmds.curve(d=2, ws=True, p=points, n=obj[:-3]+'Path')
    cmds.setAttr(cur + '.overrideEnabled', 1)
    cmds.setAttr(cur + '.overrideColor', color) 
    print cur
    cmds.move(points[0][0], points[0][1], points[0][2], cur+".scalePivot", cur+".rotatePivot", absolute=True)
    # command that runs through each cv of the curve and returns their position within a list.
    cvs = cmds.getAttr( obj[:-3]+'Path.cv[*]' )
    print cvs

    # Deletes the temp locator
    cmds.select("temp_PRT_CST")
    cmds.delete()
    cmds.select("tempLoc")
    cmds.delete()

    global initCVS
    initCVS = cvs
    # Create a locator for the motion path that the controller will now follow
    locate = cmds.spaceLocator( n=obj[:-3]+"Path_LOC" )
    #for step in range( startFrame, endFrame+2, int(curveCVstep)):
    for step in range(len(keyframes)):
        # Moves throughout the specified timeline to find point results
        cmds.currentTime( keyframes[step] )
        # Moves the locator to match the position of the controller
        cmds.move( cvs[step][0], cvs[step][1], cvs[step][2], locate)
        # Keyframes the locator
        cmds.setKeyframe( locate )
    # Position obj at the location of locate.
    cmds.pointConstraint( locate, obj, n=obj[:-3]+"LOC1_PNT" )
    # keys the weight of the point constraint to 0 before and after time frame (set to 1 during time frame)
    #Before startFrame
    cmds.currentTime( startFrame - 1 )
    cmds.setAttr(obj+'.blendPoint1', 0 )
    cmds.setKeyframe(obj+'.blendPoint1' )
    #After startframe
    cmds.currentTime( startFrame )
    cmds.setAttr(obj+'.blendPoint1', 1 )
    cmds.setKeyframe(obj+'.blendPoint1' )
    #Before endframe
    cmds.currentTime( endFrame )
    cmds.setAttr(obj+'.blendPoint1', 1 )
    cmds.setKeyframe(obj+'.blendPoint1' )
    #After endframe
    cmds.currentTime( endFrame + 1 )
    cmds.setAttr(obj+'.blendPoint1', 0 )
    cmds.setKeyframe(obj+'.blendPoint1' )
    cmds.select(obj)

Here's what the resulting motion path looks like

Here's what the correct arc should be following

Here's the new curve based on the new code. It seems to be following along the path of motion but it's compressed.