Finding the position of a pallet (blister) in a stack

vlastimil 2020-04-09

Hi,
I have a task very similar to the example "Find Contact Offset" using #ForceCopilot.

The difference is - I find for the position of the blister with the components in the stack (20 pcs).

So I can't use a simple wizard from URCap - I created my own function using the built-in "rq_functions" to find the blister position in the current stack from three points and calculated the intersection from these points (crosspoint P0).
I verified the position of the calculated by movements on this point and then by tool reference in the X and Y axes accurately copied the edges of the blister.

First, I calibrate a default position where I have teached the corner pickup points on the pallet and determine the calibration point of intersection (calibr. crosspoint K0). Then, when I rotate and move the blister, I find a new intersection of the changed position of the blister P0.

To find new corner positions of the palette on a rotated blister, I used this typical transformation:

pose_pal_curr = pose_trans(P0, pose_trans(pose_inv(K0), pose_pal_cal))

The problem is that this transformation does not work quite accurately - when the blister is shifted in the X or Y axis, it is correct;

I've already tried many variations of calculations - this transformation gives the best results however unusable.

Here is demo photo workplace:

TCP at the starting position of the first point on the palette:

TCP after transformation (see above) for the rotated and shifted palette (rotation Rz of tool is correct):

Does anyone have any idea where I make a mistake?
Thanks a lot for your advice.

Vlastimil

vlastimil 2020-04-30

So it looks like I have a problem with calculating the intersection of the three points found by touching the surface of the blister.

The method used in the "Find Contact Offset" application does not suit for me, because I do not use the wizard. I need to calculate everything with a script, because I will find out the position in different layers of a stack of blisters.

I would like to modify the Robotiq used internal functions "relocate_orientation" or "relocate_1_touch", but I'm a bit lost in it.
In case of interest I would send how I calculate the intersection.

However, the calculation is not correct, with deviations (see the pictures above).

A schematic illustration of the problem is attached:

Does anyone have experience with calculating the intersection of three points (incl. rotation)?

Thanks for the advice.

Vlastimil

vlastimil 2020-05-04

Here is a code for calculation crosspoint:

# Calculate crosspoint from three points
def cross_point(posA, posB, posC):
	pA = [posA[0], posA[1]]
	pB = [posB[0], posB[1]]
	pC = [posC[0], posC[1]]
	# directional vector line
	if posC[2]>posA[2]:
		# line AB, U = B-A
		pU = [pB[0]-pA[0], pB[1]-pA[1]]
	elif posC[2]<posB[2]:
		# line BA, U = A-B
		pU = [pA[0]-pB[0], pA[1]-pB[1]]	
	else:
                popup("The crosspoint of lines P1-P2 with the perpendicular at point P3 cannot be determined!",
                      "Error, program done!",error=True,blocking=True)
                halt	
	end
	# angle of rotation in the Z axis
	rZ = atan2(pU[0],pU[1])
	# N - Normal vector of AB line, (perpendicular to U)
	if pU[1]<0:
		pN = [-pU[1], pU[0]]
	else:
		pN = [pU[1], -pU[0]]
	end
	# a ... parametric expression of the line A-B (t, s = real numbers)
	t = 1
	s = 1
	aX = pA[0] + t * pU[0]
	aY = pA[1] + t * pU[1]
	# c ... parametric expression of the perpendicular on line A-B passing through point C
	cX = pC[0] + s * pN[0]
	cY = pC[1] + s * pN[1]
	# crosspoint of lines a x c
	pT = (pN[0]*(pC[1]-pA[1]-pN[1]*pC[0])+pN[1]*pA[0])/(pU[1]*pN[0]-pU[0]*pN[1])
	pS = ((pA[0]+pT*pU[0])/pN[0])-pC[0]
	tX = pA[0] + pT * pU[0]
	tY = pA[1] + pT * pU[1]
	# checking the correctness of the intersection of the perpendiculars
	sX = pC[0] + pS * pN[0]
	sY = pC[1] + pS * pN[1]
	# it must hold that tX = sX and tY = sY
	posT = p[tX, tY, posB[2], posB[3], posB[4], posB[5]]
	# finally transformation with Z - rotation
	return pose_trans(posT, p[0,0,0,0,0,rZ])
end

I will be happy for any advice.

V.

vlastimil 2020-05-05

In this code is mistake, here is correction:

	# directional vector line
	if posC[1]>posA[1]:
		# line AB, U = B-A
		pU = [pB[0]-pA[0], pB[1]-pA[1]]
	elif posC[1]<posB[1]:
		# line BA, U = A-B
		pU = [pA[0]-pB[0], pA[1]-pB[1]]	
	else:
                popup("The crosspoint of lines P1-P2 with the perpendicular at point P3 cannot be determined!",
                      "Error, program done!",error=True,blocking=True)
                halt	
	end

... but the problem remains.

V.

vlastimil 2020-05-05

Problem has solved, there was mistake in cross_point function, which returns the intersection of two lines from three detected points by searching the surface using a force sensor.
For everyone is functional version here:


# Calculate crosspoint from three points
def cross_point(posA, posB, posC):
	pA = [posA[0], posA[1]]
	pB = [posB[0], posB[1]]
	pC = [posC[0], posC[1]]
	# directional vector line U
	if posC[1]>posA[1]:
		# line AB, U = B-A
		pU = [pB[0]-pA[0], pB[1]-pA[1]]
	elif posC[1]<posB[1]:
		# line BA, U = A-B
		pU = [pA[0]-pB[0], pA[1]-pB[1]]	
	else:
                popup("The crosspoint of lines P1-P2 with the perpendicular at point P3 cannot be determined!",
                      "Error, program done!",error=True,blocking=True)
                halt	
	end
	# angle of rotation in the Z axis
	rZ = atan2(pU[0],pU[1])
	# N - Normal vector of AB line, (perpendicular to U)
	if pU[1]<0:
		pN = [-pU[1], pU[0]]
	else:
		pN = [pU[1], -pU[0]]
	end
	# a ... parametric expression of the line A-B (t, s = real numbers)
	t = 1
	s = 1
	aX = pA[0] + t * pU[0]
	aY = pA[1] + t * pU[1]
	# c ... parametric expression of the perpendicular on line A-B passing through point C
	cX = pC[0] + s * pN[0]
	cY = pC[1] + s * pN[1]
	# crosspoint of lines a x c
	pT = (pN[0]*(pC[1]-pA[1])+pN[1]*(pA[0]-pC[0]))/(pU[1]*pN[0]-pU[0]*pN[1])
	pS = (pA[0]+pT*pU[0]-pC[0])/pN[0]
	tX = pA[0] + pT * pU[0]
	tY = pA[1] + pT * pU[1]
	# checking the correctness of the intersection of the perpendiculars
	sX = pC[0] + pS * pN[0]
	sY = pC[1] + pS * pN[1]
	# it must hold that tX = sX and tY = sY
	posT = p[tX, tY, posB[2], posB[3], posB[4], posB[5]]
	# finally transformation with Z - rotation
	return pose_trans(posT, p[0,0,0,0,0,rZ])
end

The use of the function is clear from the above description of the problem.

So again sometime next time :smile:

Vlastimil