Program Examples:Robot tool calibration

here is an example of how to use tool calibration in case of non-existing deterministic world-frame-point. the procedure includes moving the tool to a certain position few times but with different orientations, the result of such measurments is a sphere around the tool tip. the mathematics are as follows:

point1 = {X1, Y1, Z1}

point2 = {X2, Y2, Z2}

point3 = {X3, Y3, Z3}

point4 = {X4, Y4, Z4}

shpere: Const = (X-Xc)^2 + (Y-Yc)^2 + (Z-Zc)^2

Const = (X1-Xc)^2 + (Y1-Yc)^2 + (Z1-Zc)^2

Const = (X2-Xc)^2 + (Y2-Yc)^2 + (Z2-Zc)^2

Const = (X3-Xc)^2 + (Y3-Yc)^2 + (Z3-Zc)^2

Const = (X4-Xc)^2 + (Y4-Yc)^2 + (Z4-Zc)^2

| 2(x2-x1) 2(y2-y1) 2 z2-z1) | |Xc| = | X2^2 - X1^2 + Y2^2 - Y1^2 + Z2^2 - Z1^2 |

| 2(x3-x1) 2(y3-y1) 2 z3-z1) |*|Yc| = | X3^2 - X1^2 + Y3^2 - Y1^2 + Z3^2 - Z1^2 |

| 2(x4-x1) 2(y4-y1) 2 z4-z1) | |Zc| = | X4^2 - X1^2 + Y4^2 - Y1^2 + Z4^2 - Z1^2 |

[MatA] * vecB = vecC

vecB = [MatA]^-1 * vecC

' in proto.pro import_c RBT_TOOL_CALIBRATION_SPHERE(byval as generic location, byval as generic location, byval as generic location, byval as generic location, as generic location) as long


common shared locArr[5] as generic location 

program
	
	locArr[1] = puma.setpoint
	locArr[2] = puma.setpoint
	locArr[3] = puma.setpoint
	locArr[4] = puma.setpoint
	locArr[5] = puma.setpoint
	locArr[1] = #{0,0,20,0,0,0}
	locArr[2] = #{0,10,10,0,0,0}
	locArr[3] = #{10,0,10,0,0,0}
	locArr[4] = #{0,-10,10,0,0,0}
	locArr[5] = #{0,0,0,0,0,0}

	?RBT_TOOL_CALIBRATION_SPHERE(locArr[1], locArr[2], locArr[3], locArr[4], locArr[5])
	? "sphere center: " locArr[5]

end program