Positions In Features

Technical Questions
1

I have a UR10e with a cognex camera that I am using to locate parts to pick.

The camera gives me XY and rotation of the part (rotation about RZ in camera frame).

I have 25 individual stacks of parts (in a 5x5 grid) so I have programed this in a feature (CurBladeStack) and then I move the coordinates of the feature to run the program in each different location.

CurBladeStack is rotated 90 degrees (about z) from the base coordinate system.

So I need to generate an target position for the robot to go to in CurBladeStack feature rather than in base. Currently my code to handle this is (Xpos, Ypos, Ang are the results from the camera);

def CalcFoundPose():
    #get the current pose adjust the x y and angle
    global FoundPose = get_target_tcp_pose() #get the current blade pickup position (ignore x,y and z, just orientation really)
    textmsg("TargetPose in base:", FoundPose)
    global FoundPose = pose_trans(FoundPose,CurBladeStack) # translate to the blade stack coordinates
    textmsg("CurrentTCP in CurBladeStack:",FoundPose)
    global FoundPose = p[Xpos/1000.0,Ypos/1000025.0,FoundPose[2],FoundPose[3],FoundPose[4],FoundPose[5]] # set x and y
    textmsg("XY Adjusted in CurBladeStack:",FoundPose)
    rot_rad_vec = rpy2rotvec([0,0,-d2r(Ang)])
    global FoundPose = pose_add(FoundPose,p[0,0,0,rot_rad_vec[0],rot_rad_vec[1],rot_rad_vec[2]]) # add rotation
    textmsg("Angle Adjusted in CurBladeStack:",FoundPose)
end

But my position is already incorrect by the 2nd line (CurrentTCP in CurBladeStack textmsg)

I am getting about a 0.025 radian rotation of the tool (Position seems okay).

Any idea what I am doing wrong? I am finding that programming this robot is an incredibly frustrating experience.