Universal Robots Forum

Pose_trans implementation

Hi,
I need to re-implement the pose_trans function, but I can’t figure how it really work.
I first tried to “matrix multipliate” my vector with all the rotations of the first pose argument, but results don’t match when I compare with the real function, on easy tests.
Do you use a special algorythm to calculate this ?
It’s also totally possible that I’m doing something wrong with my maths, but I have to to ask, as I lost so many time on this

(In case anybody can help me by an other way: I need to reimplement this, because I need a plan coordinates values from a URCap, I can get the plane name, its points, the coordinates of the points, but not the coordinates of the plan (the pose that appear in the “Variables” sections after program execution if we set the plane as variable), and to do this, my boss gave me a urscript program that re-create a plane based on 3 points, but I can’t send it all via the secondary client, so I need to re-implement it, and that script use a pose_trans…)

Also, I would like to know, why on the secondary client, I can’t use Installation variables? I agree that we couldn’t interfere with programs variables, but why can’t I send something like movel(Plane1,p[<some coords>]) ?

In case it help, What I’m searching for is that value, literally:

And the problem is that via urcapAPI, I can get the name of that plane, its points, and the pose of each of this points.
But not not the pose related to the plan itself. I understand that the position is the same as the first point, but I can’t get a correct to retrieve the orientation value.
I tried to make a script based on a thread on a mathematical forum, but the resulting values are not corresponding (and it’s just a 45°/90° bias, there is a problem in my maths)
I share it here in case someone can help, because I think I’m not far of a working algorythm… :weary:

def planeOrientationBy3points(p11,p12,p13,p21,p22,p23,p31,p32,p33):
	P1 = [float(p11),float(p12),float(p13)]
	P2 = [float(p21),float(p22),float(p23)]
	P3 = [float(p31),float(p32),float(p33)]

	def ps(X,Y):
	    return(X[0]*Y[0]+X[1]*Y[1]+X[2]*Y[2])

	def norme(X):
	    return((ps(X,X)**2)**0.5)

	def diff(X,Y):
	    return([Y[0]-X[0],Y[1]-X[1],Y[2]-X[2]])

	def scalarmul(a,X):
	    return([a*X[0],a*X[1],a*X[2]])

	def vectoriel(X,Y):
	    return([X[1]*Y[2]-X[2]*Y[1],X[2]*Y[0]-X[0]*Y[2],X[0]*Y[1]-X[1]*Y[0]])


	R2 = diff(P1,P2)
	R3 = diff(P1,P3)
	a = 1/norme(R2)
	i = scalarmul(a,R2)
	b = ps(i,R3)
	Q1=scalarmul(b,i)
	Q2=diff(Q1,R3)
	c=1/norme(Q2)
	j=scalarmul(c,Q2)
	k=vectoriel(i,j)
	return(str(k))

(I remove parts of code that verify if points are aligned or too close to lighten the script)

Did anyone know if I missed something ?

On the note of why you cannot work with Installation variables over the client interface;
This is due to the value of Installation variables are stored in PolyScope (and for long time keeping, in the xxxx.variables file). When a program from PolyScope is started, it includes initialization of installation variables in the pre-abmle of the program. Same thing applies to features.
Then, when the complete script program is sent to URControl. this will now know the values of these variables, and they can be used within the same program.
When the program is stopped, or no program is running, there is no variables present in URControl.

The sent program from PolyScope to Primary Client could be:

def programName():
  set_standard_analog_input_domain(0, 1)
  # Other settings from Installation file
  # ...
  set_tcp(p[0.0,0.0,0.0,0.0,0.0,0.0])
  set_payload(0.0)
  set_gravity([0.0, 0.0, 9.82])
  # Initialization of Installation variable, so it exists in URControl
  global i_var_1=0
  while (True):
    $ 1 "Robot Program"
    $ 2 "i_var_1≔i_var_1+1"
    # Able to use this variable, as it does exist in URControl
    global i_var_1=i_var_1+1
    $ 3 "Wait: 1.0"
    sleep(1.0)
  end
end

Ok, I understand. That’s almost what I concluded.
So I can’t solve my problem that way, because I have to able to travel following that plane, via a urcap button, but not launching the entire program.
I think I will ask the user the orientation of the plane with textfields, and to set the zero point, and I’ll put on the manual that he can determine the correct orientation based on a feature plane by looking at the variable section after an execution with a variable plane.
However, seem more complicate for the users than just choosing which plane in a combobox.