Universal Robots Forum

Calculate plane from 3 points

Hi!
I need to calculate a “plane pose” from 3 points the same way as a feature plane is calculated. Though I cant really figure out how to do that with the available urmath functions. Does anyone know how to do this?
The feature plane is defined in this way (cut and paste from the manual):
The position of the coordinate system is the same as the position
for the first sub point. The z-axis is the plane normal, and the y-axis is directed from
the first point towards the second. The positive direction of the z-axis is set so that the
angle between the z-axis of the plane and the z-axis of the first point is less than 180
degrees.

here u can use mine.

def global_plane_finder(p1, p2, p3):

V12 = [p2[0] - p1[0], p2[1] - p1[1], p2[2] - p1[2]]
V13 = [p3[0] - p1[0], p3[1] - p1[1], p3[2] - p1[2]]
VZ = mycross(V12, V13)
Tmat = pose2Tmat(p1)
VZofp1 = [Tmat[2], Tmat[6], Tmat[10]]
value = mydot(VZ, VZofp1)
if (value < -0.00001):
VZ = mycross(V13, V12)
end
VZ = myunitised(VZ)
VY = myunitised(V12)
VX = mycross(VY,VZ)
output = [VX[0], VY[0], VZ[0], p1[0], VX[1], VY[1], VZ[1], p1[1], VX[2], VY[2], VZ[2], p1[2], 0, 0, 0, 1]
pose = Tmat2pose(output)
return pose
end

def mycross(a, b):
output = [0, 0, 0]
output[0] = a[1]*b[2] - a[2]*b[1]
output[1] = a[2]*b[0] - a[0]*b[2]
output[2] = a[0]*b[1] - a[1]*b[0]
return output
end

def Tmat2pose(Tmat):
output = p[0, 0, 0, 0, 0, 0]
output[0] = Tmat[3]
output[1] = Tmat[7]
output[2] = Tmat[11]
sy = mynorm([Tmat[0], Tmat[4], 0])
if (sy > 0.00001):
x = atan2(Tmat[9], Tmat[10])
y = atan2(-Tmat[8], sy)
z = atan2(Tmat[4], Tmat[0])
else:
x = atan2(-Tmat[6], Tmat[5])
y = atan2(-Tmat[8], sy)
z = 0
end
rotvec = rpy2rotvec([x, y, z])
output[3] = rotvec[0]
output[4] = rotvec[1]
output[5] = rotvec[2]
return output
end

def pose2Tmat(pose):
if (pose[3]) == 0 and (pose[4] == 0) and (pose[5] == (0)):
Rmat = [1, 0, 0, 0, 1, 0, 0, 0, 1]
else:
x = pose[3]
y = pose[4]
z = pose[5]
ang = mynorm([x, y, z])
x = x/ang
y = y/ang
z = z/ang
s = sin(ang)
c = cos(ang)
t = 1 - c
Rmat = [c+(txx), (txy)-(sz), (txz)+(sy), (txy)+(sz), c+(tyy), (tyz)-(sx), (txz)-(sy), (tyz)+(sx), c+(tzz)]
end
Tmat = [Rmat[0], Rmat[1], Rmat[2], pose[0], Rmat[3], Rmat[4], Rmat[5], pose[1], Rmat[6], Rmat[7], Rmat[8], pose[2]]
return Tmat
end

def mynorm(a):
base = sqrt(a[0]*a[0] + a[1]*a[1] + a[2]*a[2])
return base
end

def myunitised(a):
output = [0, 0, 0]
base = mynorm(a)
output[0] = a[0] / base
output[1] = a[1] / base
output[2] = a[2] / base
return output
end

3 Likes

Thanks man, really appreciate it. I will look through the code, looks very similar to what I had in mind if it wasnt solvable with the built in functions.

1 Like

@elvintoh82 I tried running this but the function mydot is not defined in the code. Can you please post that function?

Thanks,
Matt

The way features are calculated from the points has changed from CB3 software v3.6 onwards and all e-Series software. If you use this method on those software versions you’ll get a different resulting plane than if you use the feature wizard.

Not necessarily a problem but something to be aware of.

https://www.universal-robots.com/how-tos-and-faqs/faq/ur-faq/release-note-software-version-36xx/

1 Like

It would be great if UR would release this ability in URScript so that it could natively solve this problem. This request has been brought up by many individuals as a way to solve for moving robots from machine to machine.

Here you go, it is basically just a simple dot product.

def mydot(a, b):
output = a[0]*b[0] + a[1]*b[1] + a[2]*b[2]
return output
end

@ajp Has UR released a way to do this with script code yet?

@elvintoh82 Firstly, thank you so much for putting this together. Second, you wouldn’t by chance have an updated version of this for CB3 series software v3.6 onwards and eSeries? I want to make use of UR’s capability to program waypoints relative to a feature, and then update that plane variable with this calculation from 3 “touchoff” points that the operator would teach.

adding @mbush as he might want to see responses.

Hi @jts, nothing officially integrated into the software yet, but one of my UR colleagues recently put together some documentation explaining various aspects of how the UR system handles coordinate transforms etc. It includes some example script functions one of which is get_feature_plane(), which do what you need for the post v3.6 planes.

1 Like

@ajp this is perfect, very informative, easy to follow, and has the code examples for the exercises

I tested it and there were only a few syntax errors in the .script file. For instance, step 7 of the get_feature_plane() function should have the assignment be to a pose variable but the ‘p’ is missing in front of the brackets.

Once those were corrected, everything works great!

great! If you found any other syntax issues please let us know. (@sba )

Hi jts, What is the code for Rotmat2rotvec function?

Hi @mkg , I got the script code from the .zip file from @ajp 's response from earlier in this thread.

Here is the link to the thread :

The robotics.zip file has a .script file with the code in it.