Script function to create a Plane with 3 variables points

Product Wishlist
1

Hi,

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.

Thanks

Charles.

2 Likes
2

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

end

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

end

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)
		else:
			ux = sqrt((r11+1)/2)
			uy = -sqrt((r22+1)/2)
			uz = sqrt((r33+1)/2)
		end
	else:
		if (r31 < 0):
			ux = sqrt((r11+1)/2)
			uy = sqrt((r22+1)/2)
			uz = -sqrt((r33+1)/2)
		else:
			ux = sqrt((r11+1)/2)
			uy = sqrt((r22+1)/2)
			uz = sqrt((r33+1)/2)
		end
	end
else:
	ux = (r32-r23)/(2*sth)
	uy = (r13-r31)/(2*sth)
	uz = (r21-r12)/(2*sth)
end

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

return rotvec

end

2 Likes
3

This is exactly what I was looking for !!

thank you

4

Heyy,

Please, I think I’m running an application similar to yours. After figuring out how to determine the new plane, I ask you how did you get the position after the robot touches the edge of your surface? How did you stop the robot at these points, which may vary. I have a very similar application, if you can help me