Home› Programming
Discussion
Back to discussions page
Finding the position of a pallet (blister) in a stack 
51 views

Answered  
/ Most recent by vlastimil
in Programming

4 comments 
vlastimil
Posts: 15 Apprentice
in Programming
Hi,
I have a task very similar to the example "Find Contact Offset" using #ForceCopilot.
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 builtin "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:
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):
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
Thanks a lot for your advice.
Vlastimil
Best Answer

vlastimil Posts: 15 ApprenticeProblem 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 = BA pU = [pB[0]pA[0], pB[1]pA[1]] elif posC[1]<posB[1]: # line BA, U = AB pU = [pA[0]pB[0], pA[1]pB[1]] else: popup("The crosspoint of lines P1P2 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 AB (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 AB 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
Vlastimil
In case of interest I would send how I calculate the intersection.
A schematic illustration of the problem is attached:
Does anyone have experience with calculating the intersection of three points (incl. rotation)?
Vlastimil
I will be happy for any advice.
V.
V.