DoF - a Robotiq Community
Warning sign
The Dof Community was shut down in June 2023. This is a read-only archive.
If you have questions about Robotiq products please reach our support team.

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.



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]]	
                popup("The crosspoint of lines P1-P2 with the perpendicular at point P3 cannot be determined!",
                      "Error, program done!",error=True,blocking=True)
	# 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]]
		pN = [pU[1], -pU[0]]
	# 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])

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

So again sometime next time :smile: