script/calibration/ScanX.py

@@ -0,0 +1,81 @@
+import plotutils
+from mathutils import fit_gaussian, Gaussian
+
+
+d = robot.get_distance_to_pnt("pLaser")
+if d<0:
+    raise Exception ("Error calculating distance to laser: " + str(d))
+
+if d>20:
+    raise Exception ("Should be near the laser position to perform the scan")
+
+RANGE = [-2.0, 2.0] #[-1.5, 1.5]
+STEP = 0.02
+Z_OFFSET = 0 #-1.0
+LATENCY = 0.025
+BORDER_SIZE = 0.15
+
+robot.enable()
+robot.set_motors_enabled(True)
+current_positon = robot_x.getPosition()
+robot_z.moveRel(Z_OFFSET)
+
+robot.setPolling(25)
+try:
+    ret = lscan(robot_x, ue.readable, RANGE[0], RANGE[1], STEP, latency = LATENCY, relative = True)
+finally:    
+    robot.setPolling(DEFAULT_ROBOT_POLLING)
+
+d = ret.getReadable(0)
+
+first_index = -1
+last_index = -1
+for i in range(len(d)):
+    if not math.isnan(d[i]):
+        if first_index<0:
+            first_index = i  
+        last_index = i  
+
+if first_index == -1 or last_index < first_index:
+    raise Exception("Invalid range")
+
+
+remove = int(max(BORDER_SIZE, STEP) / STEP)
+
+_range = [first_index+remove, last_index-remove]
+if _range[1] <= _range[0]:
+    raise Exception("Invalid range: " + str(_range))
+
+
+center_index = int((_range[0] + _range[1])/2)
+center_positon = ret.getPositions(0)[center_index]
+
+
+
+
+y =  ret.getReadable(0)[_range[0] : _range[1]]
+x =  ret.getPositions(0)[_range[0]: _range[1]]
+#x = enforce_monotonic(x)
+#(normalization, mean_val, sigma) = fit_gaussian([-v for v in y], x)
+
+
+closest_x = x[y.indexOf(min(y))]
+closest_y = y[y.indexOf(min(y))]
+
+
+
+if closest_x is None or closest_x <= ret.getPositions(0)[first_index] or closest_x >= ret.getPositions(0)[last_index]:
+    raise Exception("Invalid Fit")
+
+
+
+center_offset = center_positon-closest_y
+#center_offset = current_positon-closest_y
+
+
+
+p=get_plots()[0]
+p.addMarker(closest_y, p.AxisId.X, str(closest_y), Color.GREEN)
+
+robot.set_motors_enabled(True)
+robot_x.move(closest_x)

script/calibration/ScanY.py

@@ -37,14 +37,14 @@ for i in range(len(d)):
         last_index = i  
 
 if first_index == -1 or last_index < first_index:
-    raise Exception("Invalid X detection")
+    raise Exception("Invalid range")
 
 
 remove = int(max(BORDER_SIZE, STEP) / STEP)
 
 _range = [first_index+remove, last_index-remove]
 if _range[1] <= _range[0]:
-    raise Exception("invalid range: " + str(_range))
+    raise Exception("Invalid range: " + str(_range))
 
 
 center_index = int((_range[0] + _range[1])/2)

script/calibration/ToolCalibration3.py

@@ -0,0 +1,65 @@
+import plotutils
+from mathutils import fit_gaussian, Gaussian
+
+cal_tool = TOOL_CALIBRATION
+
+robot.set_tool(cal_tool)
+robot.enable()
+move_to_laser()
+
+
+robot.set_motors_enabled(True)
+robot.set_joint_motors_enabled(True)
+initial_pos = robot.get_cartesian_pos()
+
+#robot.align()  
+
+run("calibration/ScanX")
+
+pos1 = robot.get_cartesian_pos()
+x1, l1 = closest_x, closest_y
+
+print "Closest 1: ", [x1, l1]
+print "Position 1: ", pos1
+
+
+pj6 = robot_j6.position
+if pj6>0:
+    robot_j6.move(pj6 - 180.0)
+else:    
+    robot_j6.move(pj6 + 180.0)
+
+
+run("calibration/ScanX")
+
+
+x2, l2 = closest_x, closest_y
+pos2 = robot.get_cartesian_pos()
+
+print "Closest 2: ", [x2, l2]
+print "Position 2: ", pos2
+
+
+#Updates the tool
+t=robot.get_tool_trsf(TOOL_DEFAULT)
+
+xoff = (x1-x2)/2
+yoff = (l2 - l1)/2 
+xrot = math.degrees(math.atan(yoff/t[2]))
+yrot = math.degrees(math.atan(xoff/t[2]))
+
+print "Former tool: " + str(t)
+t[0]=xoff 
+t[1]=-yoff 
+print "Offset: ", [t[0], t[1]]
+
+print "CALIBRATED tool: " + str(t)
+#robot.set_tool_trsf(t, TOOL_DEFAULT)
+
+robot.set_tool(TOOL_DEFAULT)
+d = robot.get_distance_to_pnt("pLaser")
+if d<POSITION_TOLERANCE:
+    print "Moving calibrated tool to laser"
+    #move_to_laser()    
+else:
+    print "Cannot move calibrated tool to laser: too big offset"