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+(t*x*x), (t*x*y)-(s*z), (t*x*z)+(s*y), (t*x*y)+(s*z), c+(t*y*y), (t*y*z)-(s*x), (t*x*z)-(s*y), (t*y*z)+(s*x), c+(t*z*z)]

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

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.

@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/

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.

@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!

Hi jts, What is the code for Rotmat2rotvec function?