# Script function to create a Plane with 3 variables points

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.

Something like this:
from the post

# p3: determines sign of Y axis

def get_feature_plane(p1, p2, p3):

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

# 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/temp, d12/temp, d12/temp ]
temp = norm(dz)
uz = [ dz/temp, dz/temp, dz/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, ux, ux, uy, uy, uy, uz, uz, uz ]

# 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, p1, p1, rotvec, rotvec, rotvec ]

return feature_plane
``````

end

# cross product of two vectors u and v

def cross_product(u, v):

``````u1 = u
u2 = u
u3 = u
v1 = v
v2 = v
v3 = v

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
r21 = rotmat
r31 = rotmat
r12 = rotmat
r22 = rotmat
r32 = rotmat
r13 = rotmat
r23 = rotmat
r33 = rotmat

# 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

This is exactly what I was looking for !!

thank you