script/calibration/BinarySearchYZ.py

@@ -15,15 +15,15 @@ move_to_laser()
 
 
 robot.set_motors_enabled(True)
-current_x = robot_x.getPosition()
+current_y = robot_y.getPosition()
 current_z = robot_z.getPosition()
 
 
-r = bsearch([robot_x, robot_z], laser_distance,[RANGE[0], RANGE[0]], [RANGE[1], RANGE[1]], [STEP_SIZE,STEP_SIZE], relative = True, maximum=True, strategy = STRATEGY, latency = LATENCY, title = "Binary Search XZ")
+r = bsearch([robot_y, robot_z], laser_distance,[RANGE[0], RANGE[0]], [RANGE[1], RANGE[1]], [STEP_SIZE,STEP_SIZE], relative = True, maximum=True, strategy = STRATEGY, latency = LATENCY, title = "Binary Search YZ")
 
 
 print r.print()
-opt_x, opt_z= r.getOptimalPosition()
-offset_x, offset_z = opt_x - current_x, opt_z - current_z
+opt_y, opt_z= r.getOptimalPosition()
+offset_y, offset_z = opt_y - current_y, opt_z - current_z
 
-print "offset_x: ", offset_x, " offset_z: ", offset_z
+print "offset_y: ", offset_y, " offset_z: ", offset_z

script/calibration/ScanRZ.py

@@ -1,9 +1,13 @@
 import plotutils
 from mathutils import fit_gaussian, Gaussian
 
-RANGE = [-20.0,20.0]
-STEP = 1.0
+RANGE = [180.0,-180.0]
+STEP = 5.0
 LATENCY = 0.005
+RELATIVE = False
+
+
+robot.set_tool(TOOL_DEFAULT)
 
 robot.enable()
 move_to_laser()
@@ -13,8 +17,9 @@ move_to_laser()
 
 
 
-robot.set_motors_enabled(True)
-ret = lscan(robot_rz, ue.readable, RANGE[0], RANGE[1], STEP, latency = LATENCY, relative = True, range = "auto")
+
+robot.set_joint_motors_enabled(True)
+ret = lscan(robot_rz, ue.readable, RANGE[0], RANGE[1], STEP, latency = LATENCY, relative = RELATIVE, range = "auto", title = "Scan2")
 
 
 

script/calibration/ScanX.py

@@ -1,37 +0,0 @@
-import plotutils
-from mathutils import fit_gaussian, Gaussian
-
-robot.enable()
-move_to_laser()
-
-
-RANGE = [-1.5, 1.5]
-STEP = 0.025
-Z_OFFSET = -1.0
-LATENCY = 0.005
-
-robot.set_motors_enabled(True)
-current_positon = robot_x.getPosition()
-robot_z.moveRel(Z_OFFSET)
-ret = lscan(robot_x, ue.readable, RANGE[0], RANGE[1], STEP, latency = LATENCY, relative = True)
-
-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 X detection")
-
-
-center_index = (first_index + last_index)/2
-center_positon = ret.getPositions(0)[center_index]
-center_offset = center_positon-current_positon
-
-
-print "X offset = ", center_offset

script/calibration/ScanY.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 = [-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_y.getPosition()
+robot_z.moveRel(Z_OFFSET)
+
+robot.setPolling(25)
+try:
+    ret = lscan(robot_y, 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 X detection")
+
+
+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_y = x[y.indexOf(min(y))]
+closest_x = y[y.indexOf(min(y))]
+
+
+
+if closest_y is None or closest_y <= ret.getPositions(0)[first_index] or closest_y >= 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_y.move(closest_y)

script/calibration/ScanYZ.py

@@ -21,24 +21,24 @@ STEP_SIZE = 0.1
 robot.enable()
 move_to_laser()
 
-step_x = STEP_SIZE
+step_y = STEP_SIZE
 step_z = STEP_SIZE
-range_x = [RANGE[0], RANGE[1]]
+range_y = [RANGE[0], RANGE[1]]
 range_z = [RANGE[0], RANGE[1]]
 
 robot.set_motors_enabled(True)
-current_x = robot_x.getPosition()
+current_y = robot_y.getPosition()
 current_z = robot_z.getPosition()
 
-print "Current pos x,z" , current_x, ",", current_z
-ret = ascan([robot_x, robot_z], ue.readable, [range_x[0], range_z[0]], [range_x[1], range_z[1]], [step_x,step_z], latency = LATENCY, relative = True , zigzag=False, title = "Scan XY")
+print "Current pos y,z" , current_y, ",", current_z
+ret = ascan([robot_y, robot_z], ue.readable, [range_y[0], range_z[0]], [range_y[1], range_z[1]], [step_y,step_z], latency = LATENCY, relative = True , zigzag=False, title = "Scan XY")
 data = ret.getData(0)[0]
 #plot(Convert.transpose(data), title="Data")
 
 integ = []
 for x in data: integ.append(sum(   [ (0.0 if (math.isnan(y)) else y)  for y  in x]))
 
-xdata= frange(range_x[0], range_x[1], step_x , False, True)
+xdata= frange(range_y[0], range_y[1], step_y , False, True)
 p = plot(integ, title = "Fit", xdata=xdata)[0]
 
 
@@ -49,18 +49,18 @@ try:
 except:
     raise Exception("Invalid Fit")    
 gaussian = Gaussian(normalization, mean_val, sigma)
-xdata= frange(range_x[0], range_x[1], step_x/100.0 , False, True)
+xdata= frange(range_y[0], range_y[1], step_y/100.0 , False, True)
 plot_function(p, gaussian, "Fit", xdata, show_points = False, show_lines = True, color = Color.BLUE)
 
 #So 
 if abs(mean_val - max_x) > 1.0:
-    raise Exception("Invalid X detection")
-x_offset = mean_val    
-center_x = current_x + x_offset
+    raise Exception("Invalid Y detection")
+y_offset = mean_val    
+center_y = current_y + y_offset
 
-print "X offset = ", x_offset
+print "Y offset = ", y_offset
 
-robot_x.move(center_x)
+robot_y.move(center_y)
 if SINGLE_PASS:
     z_scan_data = data[max_x_index]        
 else:    

script/calibration/ToolCalibration.py

@@ -15,17 +15,17 @@ move_to_laser()
 robot.align()  
 
 
-run("calibration/ScanXZ")
+run("calibration/ScanYZ")
 
 robot.set_motors_enabled(True)
 
 
-first_x = robot_x.take()
+first_y = robot_y.take()
 first_z = robot_z.take()
 first_y = ue.take()
 first_j6 = robot_j6.take()
 if first_y is None:
-    raise Exception("Invalid XZ scan values in first scan")
+    raise Exception("Invalid YZ scan values in first scan")
 
 
 robot.set_joint_motors_enabled(True)
@@ -36,11 +36,11 @@ else:
 
     
 robot.set_motors_enabled(True)
-run("calibration/ScanXZ")
+run("calibration/ScanYZ")
 
 robot.set_motors_enabled(True)
 
-second_x = robot_x.take()
+second_y = robot_y.take()
 second_z = robot_z.take()
 second_y = ue.take()
 second_j6 = robot_j6.take()
@@ -51,10 +51,10 @@ if second_y is None:
 #Updates the tool
 xoff = (first_x - second_x)/2
 yoff = (first_y - second_y)/2
-robot.get_tool_trsf(robot.tool)
-t[0]=xoff     #TODO: Why signal?
-t[1]=-yoff    #TODO: Why signal?
-robot.set_tool_trsf(t, robot.tool)
+t=robot.get_tool_trsf(TOOL_DEFAULT)
+t[0]=xoff 
+t[1]=-yoff 
+robot.set_tool_trsf(t, TOOL_DEFAULT)
 
 
 

script/calibration/ToolCalibration2.py

@@ -0,0 +1,75 @@
+import plotutils
+from mathutils import fit_gaussian, Gaussian
+
+
+robot.assert_tool(TOOL_CALIBRATION)
+robot.enable()
+robot.set_motors_enabled(True)
+robot.set_joint_motors_enabled(True)
+move_to_laser()
+
+
+initial_pos = robot.get_cartesian_pos()
+
+robot.enable()
+move_to_laser()
+
+#robot.align()  
+
+
+run("calibration/ScanY")
+
+pos1 = robot.get_cartesian_pos()
+x1, y1 = closest_x, closest_y
+
+print "Closest 1: ", [x1, y1]
+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/ScanY")
+
+
+x2, y2 = closest_x, closest_y
+
+print "Closest 2: ", [x2, y2]
+
+
+
+off_x = x1 - x2
+
+#robot.set_motors_enabled(True)
+#robot_x.moveRel(off_x, -1)
+
+#For composing cannot use tcp_p, need another auxiliary point. tcp_t is also destroyed.
+robot.set_pnt(robot.get_cartesian_pos(), "pTemp")
+robot.set_trsf([off_x, 0,0,0,0,0])
+c=robot.compose("pTemp", "fTable", "tcp_t" )
+robot.set_pnt(c, "pTemp")
+robot.movel("pTemp", TOOL_CALIBRATION, DESC_SCAN, sync=True)
+
+
+pos2 = robot.get_cartesian_pos()
+print pos2
+
+print "Position 2: ", pos2
+
+
+
+#Updates the tool
+xoff = (pos2[0]-pos1[0])/2
+yoff = (pos2[1]-pos1[1])/2
+
+print "Offset: ", [xoff, yoff]
+
+t=robot.get_tool_trsf(TOOL_DEFAULT)
+t[0]=-xoff 
+t[1]=yoff 
+robot.set_tool_trsf(t, TOOL_DEFAULT)
+