script/calibration/ScanXZ.py

@@ -3,14 +3,19 @@ from mathutils import fit_gaussian, Gaussian
 
 SINGLE_PASS = False
 if SINGLE_PASS:
-    STEP_SIZE = 0.5
+    STEP_SIZE = 0.2
 else:
     STEP_SIZE = 1.0
+STEP_Z_FINAL = 0.1
+    
 RANGE = [-5.0, 5.0]
 LATENCY = 0.005
 
+Z_FINAL_OFFSET = 0.0
+
+
 robot.enable()
-move_to_laser()
+#move_to_laser()
 
 step_x = STEP_SIZE
 step_z = STEP_SIZE
@@ -22,7 +27,7 @@ current_x = robot_x.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)
+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 = "2d Scan")
 data = ret.getData(0)[0]
 #plot(Convert.transpose(data), title="Data")
 
@@ -35,8 +40,12 @@ p = plot(integ, title = "Fit", xdata=xdata)[0]
 
 max_x_index = integ.index(max(integ))
 max_x = xdata[max_x_index]
-(normalization, mean_val, sigma) = fit_gaussian(integ, xdata)
+try:
+    (normalization, mean_val, sigma) = fit_gaussian(integ, xdata)
+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)
 plot_function(p, gaussian, "Fit", xdata, show_points = False, show_lines = True, color = Color.BLUE)
 
 #So 
@@ -47,12 +56,11 @@ center_x = current_x + x_offset
 
 print "X offset = ", x_offset
 
-
+robot_x.move(center_x)
 if SINGLE_PASS:
     z_scan_data = data[max_x_index]        
-else:
-    robot_x.move(center_x)
-    step_z = step_z/4.0
+else:    
+    step_z = STEP_Z_FINAL
     ret2 = lscan(robot_z, ue.readable, range_z[0], range_z[1], step_z, latency = LATENCY, relative = True , zigzag=False)
     z_scan_data = ret2.getData(0)[0]
     
@@ -70,7 +78,9 @@ if SINGLE_PASS:
 else:    
     max_z = ret2.getPositions(0)[last_z_index]    
 
-z_offset = max_z-current_z
+z_offset = max_z - current_z + Z_FINAL_OFFSET
+
+robot_z.move(max_z + Z_FINAL_OFFSET)
 
 print "Z offset = ", z_offset
 

script/calibration/ToolCalibration.py

@@ -2,41 +2,59 @@ import plotutils
 from mathutils import fit_gaussian, Gaussian
 
 
-robot.enable()
-move_to_laser()
+robot.assert_tool(TOOL_CALIBRATION)
 robot.set_motors_enabled(True)
+robot.set_joint_motors_enabled(True)
 
 
 initial_pos = robot.get_cartesian_pos()
 
+robot.enable()
+move_to_laser()
+
+robot.align()  
 
 
 run("calibration/ScanXZ")
 
 robot.set_motors_enabled(True)
-robot_x.moveRel(x_offset)
-robot_z.moveRel(z_offset)
 
 
-initial_x = robot_x.take()
-initial_z = robot_z.take()
-initial_y = ue.take()
-if initial_y is None:
-    raise Exception("Invalid XZ scan values")
+first_x = robot_x.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")
+
+
+robot.set_joint_motors_enabled(True)
+if first_j6>0:
+    robot_j6.moveRel(-180.0, -1)
+else:
+    robot_j6.moveRel(180.0, -1)    
+
+    
+robot.set_motors_enabled(True)
+run("calibration/ScanXZ")
+
+robot.set_motors_enabled(True)
+
+second_x = robot_x.take()
+second_z = robot_z.take()
+second_y = ue.take()
+second_j6 = robot_j6.take()
+if second_y is None:
+    raise Exception("Invalid XZ scan values in first scan")
+
+
+#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)
 
 
 
-
-
-
-#update_tool(None, x_offset=x_offset, z_offset=z_offset)
-
-current_x = robot_x.getPosition()
-current_y = robot_x.getPosition()
-current_z = robot_z.getPosition()
-
-
-
-#Updating tool:
-#update_tool(None, x_offset=x_offset, z_offset=z_offset)
-