Plane defined by distance sensors

Technical Questions
1

I’m working on a project with a UR robot and three distance sensors mounted around the TCP. Each sensor only provides the distance to a wall. I want to use these measurements to define a plane representing the wall and then have the robot move along a plane offset by a fixed distance (e.g., 20 cm) from the wall. I’m trying to figure out how to convert the sensor distances into 3D points and then calculate a plane pose suitable for the robot to follow.

I tried to use this script

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+(txx), (txy)-(sz), (txz)+(sy), (txy)+(sz), c+(tyy), (tyz)-(sx), (txz)-(sy), (tyz)+(sx), c+(tzz)]
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

But i dont know how to transform signals from my distance sensors located near to TCP into 3D points positions. Thank you in advance.

2

Is that a ChatGPT generated script? :slight_smile:

The command get_feature_plane_pose is not a URScript command, from what I can tell.
I’m not aware of any such command, either.

I recalled seeing the subject before, so I found this thread: Calculate plane from 3 points - Technical Questions / URScript - Universal Robots Forum

Maybe it can guide you further.

3

Thank you for your answer, I saw this thread before and i was trying to use code which is shown there, but i dont know how to transform the signals from my distance sensors, which are located near the TCP, into 3D positions that the robot can read when searching for a plane.

4

If you make sure to point the distance sensors as close to an axis as possible, then it should be fairly simple.

If, for example, they’re pointed in the X direction, you can just offset the positions in the X direction according to the sensors.

If they’re not parallel to an axis, then it’s going to be more difficult. You’ll have to offset the points with regard to the Tool coordinates, and then translate it to the base feature before making the plane.

1 Like
5

Basically, you need to get data from sensor (assume you are getting x and z), and add those to pre-measured (for example, when directed on a flat surface while TCP is toucing the surface) sensor offset - x, z, y when applies and angle between sensor’s ray and tcp.

1 Like