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

# 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

This is exactly what I was looking for !!

thank you