Universal Robots Forum

Script function to create a Plane with 3 variables points


I’m looking for a way to calculate a plane using 3 variables points to allow the robot to reteach a plane which already exist.

My problem is that i’m using an automatic motorized robot (AMR) which can’t be precise when it stop. My robot has to place Parts on this AMR. So I want the robot to “touch” the edge of the AMR and calculate the new plane using those points.



Something like this:
from the post

get feature plane from three points

p1: origin

p2: determines positive X axis (direction from p1 to p2 is X axis)

p3: determines sign of Y axis

def get_feature_plane(p1, p2, p3):

# Step 1. Get the direction vectors
d12 = [ p2[0]-p1[0], p2[1]-p1[1], p2[2]-p1[2] ]
d13 = [ p3[0]-p1[0], p3[1]-p1[1], p3[2]-p1[2] ]

# Step 2. Get the direction vector of Z axis by cross product of d12 and d13
dz = cross_product(d12, d13)

# Step 3. Get the X and Z unit direction vectors by normalizing d12 and dz
temp = norm(d12)
ux = [ d12[0]/temp, d12[1]/temp, d12[2]/temp ]
temp = norm(dz)
uz = [ dz[0]/temp, dz[1]/temp, dz[2]/temp ]

# Step 4. Get Y unit direction vector by cross product of uz and ux
uy = cross_product(uz, ux)

# Step 5. Get the rotation matrix from the unit direction vectors
rotmat = [ ux[0], ux[1], ux[2], uy[0], uy[1], uy[2], uz[0], uz[1], uz[2] ]

# Step 6. Get the rotation vector from the rotation matrix
rotvec = rotmat2rotvec(rotmat)

# Step 7. Get the feature plane with the origin at p1 and the frame achieved at step 6
feature_plane = [ p1[0], p1[1], p1[2], rotvec[0], rotvec[1], rotvec[2] ]

return feature_plane


cross product of two vectors u and v

def cross_product(u, v):

u1 = u[0]
u2 = u[1]
u3 = u[2]
v1 = v[0]
v2 = v[1]
v3 = v[2]

s = [ (u2*v3-u3*v2), (u3*v1-u1*v3), (u1*v2-u2*v1) ]

return s


convert from rotation matrix to rotation vector

def rotmat2rotvec(rotmat):

# array to matrix
r11 = rotmat[0]
r21 = rotmat[1]
r31 = rotmat[2]
r12 = rotmat[3]
r22 = rotmat[4]
r32 = rotmat[5]
r13 = rotmat[6]
r23 = rotmat[7]
r33 = rotmat[8]

# rotation matrix to rotation vector
theta = acos((r11+r22+r33-1)/2)
sth = sin(theta)

if ( (theta > d2r(179.99)) or (theta < d2r(-179.99)) )
	theta = d2r(180)
	if (r21 < 0):
		if (r31 < 0):
			ux = sqrt((r11+1)/2)
			uy = -sqrt((r22+1)/2)
			uz = -sqrt((r33+1)/2)
			ux = sqrt((r11+1)/2)
			uy = -sqrt((r22+1)/2)
			uz = sqrt((r33+1)/2)
		if (r31 < 0):
			ux = sqrt((r11+1)/2)
			uy = sqrt((r22+1)/2)
			uz = -sqrt((r33+1)/2)
			ux = sqrt((r11+1)/2)
			uy = sqrt((r22+1)/2)
			uz = sqrt((r33+1)/2)
	ux = (r32-r23)/(2*sth)
	uy = (r13-r31)/(2*sth)
	uz = (r21-r12)/(2*sth)

rotvec = [(theta*ux),(theta*uy),(theta*uz)]

return rotvec


This is exactly what I was looking for !!

thank you