Shifting Direction Based on Taught Waypoint(s)

Hi all,

First thing, I've scowered these forums the last few weeks in an attempt to learn URScript- you guys are truly amazing!

I've been developing a program that uses relative movements, using pose_trans to "add" shifts of p[dx,0,0,0,0,0] or p[dx,dy,0,0,0,0], for example, on an Initial position (that we intend to teach manually).

An example:

global p_ileft = get_target_tcp_pose() #def initial pose

global p_via = p[0.5,1,0,0,0,0]

global p_to = p[1,0,0,0,0,0]

global p_iCenter = pose_trans(p_ileft, p_via) #def center pose

global p_iright = pose_trans(p_ileft, p_to) #def end pose

movec(p_iCenter, p_iright) #MoveC around semicircle

stopl()

sleep(0.01)

movec(p_iCenter, p_ileft) #MoveC back around semicircle

stopl()

sleep(0.01)

We'd like to teach each initial rod's initial position (p_ileft above), and have the UR orient its movements based on the rotation between initial (of that rod) waypoints. For example, if we think of the bottom left rod as the original orientation, then the top center one will be shifted 90 degrees (so our "x" will now be vertical from the viewer's perspective). Unfortunately these shifts aren't exactly 90 deg., and we need to be able to run the relative movement scripts from any x-y orientation.

I made a script that (successfully) takes the difference in angles between waypoints and adds them to the initial TCP- defined in the Installation (and manually created into a variable in BeforeStart):

WP_Old = get_target_tcp_pose()

#move to new waypoint

WP_New = get_target_tcp_pose()

p_TCP = TCP_defined #Defined in BeforeStart (the Installation TCP)- currently using p[x,y,z,0,0,0]

#Take differences in Waypoints to find orientation difference

p_TCP_difference = pose_sub(WP_New, WP_Old)

p_TCP_shift = p[0, 0, 0, p_TCP_difference[3], p_TCP_difference[4], p_TCP_difference[5]] #Take only the angles from the TCP Difference

global p_newTCP = pose_trans(p_TCP, p_TCP_shift)

sleep(0.01)

set_tcp(p_newTCP)

The issue is, however, that these translations aren't correct, since get_target_tcp_pose (and the other ways of pulling the pose value, from what I can tell...) is WRT the base frame, not the flange frame (which the TCP rotation's are WRT).

So- is this a reasonable approach to take in the first place? If so, how can I find the rotations WRT the flange, rather than the base, so I can use them for the TCP shift?

And if not, what's the correct approach? I've seen several references to using a Plane Feature to determine TCP alignment, and have tried that method, with no luck. How would we use the plane to determine the angles to shift by?

Thank you in advance for any help- and like I mentioned before, I'm so grateful for this wealth of knowledge on UR scripting. I'd be a lost cause without y'all!