Startup
This commit is contained in:
gac-S_Changer
@@ -0,0 +1,29 @@
|
||||
import plotutils
|
||||
import math
|
||||
|
||||
|
||||
#STRATEGY = "Normal"
|
||||
#STRATEGY = "Boundary"
|
||||
#STRATEGY = "FullNeighborhood"
|
||||
RANGE = [-5.0, 5.0]
|
||||
STEP_SIZE = 0.1
|
||||
LATENCY = 0.05
|
||||
|
||||
|
||||
robot.enable()
|
||||
move_to_laser()
|
||||
|
||||
|
||||
robot.set_motors_enabled(True)
|
||||
current_y = robot_y.getPosition()
|
||||
current_z = robot_z.getPosition()
|
||||
|
||||
|
||||
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_y, opt_z= r.getOptimalPosition()
|
||||
offset_y, offset_z = opt_y - current_y, opt_z - current_z
|
||||
|
||||
print "offset_y: ", offset_y, " offset_z: ", offset_z
|
||||
@@ -0,0 +1,45 @@
|
||||
import plotutils
|
||||
import math
|
||||
|
||||
|
||||
#STRATEGY = "Normal"
|
||||
STRATEGY = "Boundary"
|
||||
#STRATEGY = "FullNeighborhood"
|
||||
RANGE = [-5.0, 5.0]
|
||||
INITIAL_STEP = 1.0
|
||||
STEP_SIZE = 0.05
|
||||
LATENCY = 0.05
|
||||
NOISE_FILTER = 1
|
||||
|
||||
|
||||
robot.enable()
|
||||
move_to_laser()
|
||||
|
||||
|
||||
robot.set_motors_enabled(True)
|
||||
|
||||
current_x = robot_x.getPosition()
|
||||
current_z = robot_z.getPosition()
|
||||
|
||||
|
||||
|
||||
|
||||
class Distance(Readable):
|
||||
def read(self):
|
||||
ret = ue.readable.read()
|
||||
ret = 0.0 if math.isnan(ret) else ret
|
||||
return ret
|
||||
|
||||
laser_distance=Distance()
|
||||
start = time.time()
|
||||
r = hsearch([robot_x, robot_z],laser_distance, [RANGE[0], RANGE[0]], [RANGE[1], RANGE[1]], [INITIAL_STEP,INITIAL_STEP], [STEP_SIZE,STEP_SIZE], NOISE_FILTER, relative = True, maximum=True, latency = LATENCY, title = "Hill Climbing XZ")
|
||||
|
||||
|
||||
|
||||
print r.print()
|
||||
opt_x, opt_z= r.getOptimalPosition()
|
||||
offset_x, offset_z = opt_x - current_x, opt_z - current_z
|
||||
|
||||
print "offset_x: ", offset_x, " offset_z: ", offset_z
|
||||
|
||||
|
||||
@@ -0,0 +1,31 @@
|
||||
#Imports
|
||||
import plotutils
|
||||
from mathutils import fit_gaussian, Gaussian
|
||||
|
||||
#Parameters
|
||||
RANGE = [-120.0,120.0]
|
||||
STEP = 5.0
|
||||
LATENCY = 0.005
|
||||
RELATIVE = False
|
||||
|
||||
#Enabling and checking
|
||||
#enable_motion()
|
||||
#system_check()
|
||||
robot.enable()
|
||||
|
||||
|
||||
#Body
|
||||
robot.set_tool(TOOL_DEFAULT)
|
||||
move_to_laser()
|
||||
#robot.set_joint_motors_enabled(True)
|
||||
robot.set_motors_enabled(True)
|
||||
robot_rz.move(0.0)
|
||||
robot.set_motors_enabled(True)
|
||||
ret = lscan(robot_rz, ue.readable, RANGE[0], RANGE[1], STEP, latency = LATENCY, relative = RELATIVE, range = "auto", title = "Scan2")
|
||||
|
||||
|
||||
#Cleanup
|
||||
|
||||
|
||||
|
||||
|
||||
@@ -0,0 +1,101 @@
|
||||
import plotutils
|
||||
from mathutils import fit_gaussian, Gaussian
|
||||
|
||||
FIT = True
|
||||
|
||||
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 = [-5.0, 4.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)
|
||||
|
||||
print "Moving to scan start position: " , RANGE[0]
|
||||
robot.set_motors_enabled(True)
|
||||
robot_x.moveRel( RANGE[0])
|
||||
robot.set_motors_enabled(True)
|
||||
print "Starting scan X"
|
||||
|
||||
RANGE = [0, (RANGE[1] - RANGE[0] )]
|
||||
|
||||
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]]
|
||||
|
||||
#Clear NaNs
|
||||
first_value=ret.getReadable(0)[first_index]
|
||||
y = [(first_value if math.isnan(v) else v) for v in y]
|
||||
|
||||
|
||||
|
||||
if FIT:
|
||||
x = enforce_monotonic(x)
|
||||
offset=100
|
||||
(normalization, mean_val, sigma) = fit_gaussian([offset-v for v in y], x)
|
||||
closest_x = mean_val
|
||||
closest_y = 100 -normalization
|
||||
else:
|
||||
closest_x = x[y.index(min(y))]
|
||||
closest_y = y[y.index(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_x, p.AxisId.X, str(closest_x), Color.GREEN)
|
||||
|
||||
robot.set_motors_enabled(True)
|
||||
robot_x.move(closest_x)
|
||||
@@ -0,0 +1,99 @@
|
||||
import plotutils
|
||||
from mathutils import fit_gaussian, Gaussian
|
||||
|
||||
FIT = True
|
||||
|
||||
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 = [-7.0, 4.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_y.getPosition()
|
||||
robot_z.moveRel(Z_OFFSET)
|
||||
|
||||
|
||||
print "Moving to scan start position: " , RANGE[0]
|
||||
robot.set_motors_enabled(True)
|
||||
robot_y.moveRel( RANGE[0])
|
||||
robot.set_motors_enabled(True)
|
||||
print "Starting scan Y"
|
||||
|
||||
RANGE = [0, (RANGE[1] - RANGE[0] )]
|
||||
|
||||
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 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]]
|
||||
|
||||
#Clear NaNs
|
||||
first_value=ret.getReadable(0)[first_index]
|
||||
y = [(first_value if math.isnan(v) else v) for v in y]
|
||||
|
||||
|
||||
if FIT:
|
||||
x = enforce_monotonic(x)
|
||||
offset=100
|
||||
(normalization, mean_val, sigma) = fit_gaussian([offset-v for v in y], x)
|
||||
closest_y = mean_val
|
||||
closest_x = 100 -normalization
|
||||
else:
|
||||
closest_y = x[y.index(min(y))]
|
||||
closest_x = y[y.index(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)
|
||||
@@ -0,0 +1,93 @@
|
||||
import plotutils
|
||||
from mathutils import fit_gaussian, Gaussian
|
||||
|
||||
SINGLE_PASS = False
|
||||
if SINGLE_PASS:
|
||||
STEP_SIZE = 0.2
|
||||
else:
|
||||
STEP_SIZE = 1.0
|
||||
STEP_Z_FINAL = 0.1
|
||||
|
||||
RANGE = [-5.0, 5.0]
|
||||
LATENCY = 0.05
|
||||
|
||||
Z_FINAL_OFFSET = 0.0
|
||||
|
||||
|
||||
SINGLE_PASS = True
|
||||
STEP_SIZE = 0.1
|
||||
|
||||
|
||||
robot.enable()
|
||||
move_to_laser()
|
||||
|
||||
step_y = STEP_SIZE
|
||||
step_z = STEP_SIZE
|
||||
range_y = [RANGE[0], RANGE[1]]
|
||||
range_z = [RANGE[0], RANGE[1]]
|
||||
|
||||
robot.set_motors_enabled(True)
|
||||
current_y = robot_y.getPosition()
|
||||
current_z = robot_z.getPosition()
|
||||
|
||||
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_y[0], range_y[1], step_y , False, True)
|
||||
p = plot(integ, title = "Fit", xdata=xdata)[0]
|
||||
|
||||
|
||||
max_x_index = integ.index(max(integ))
|
||||
max_x = xdata[max_x_index]
|
||||
try:
|
||||
(normalization, mean_val, sigma) = fit_gaussian(integ, xdata)
|
||||
except:
|
||||
raise Exception("Invalid Fit")
|
||||
gaussian = Gaussian(normalization, mean_val, sigma)
|
||||
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 Y detection")
|
||||
y_offset = mean_val
|
||||
center_y = current_y + y_offset
|
||||
|
||||
print "Y offset = ", y_offset
|
||||
|
||||
robot_y.move(center_y)
|
||||
if SINGLE_PASS:
|
||||
z_scan_data = data[max_x_index]
|
||||
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]
|
||||
|
||||
max_z_index= z_scan_data.index(max(z_scan_data))
|
||||
last_z_index = 0
|
||||
for i in range(len(z_scan_data)):
|
||||
if not math.isnan(z_scan_data[i]):
|
||||
last_z_index = i
|
||||
#Shape is cone: z is inceraseing. For proper detection last Z must be furthest
|
||||
if abs(max_z_index - last_z_index) * step_z > 1.0:
|
||||
raise Exception("Invalid Z detection")
|
||||
|
||||
if SINGLE_PASS:
|
||||
max_z = ret.getPositions(1)[len(data[0]) * max_x_index + last_z_index]
|
||||
else:
|
||||
max_z = ret2.getPositions(0)[last_z_index]
|
||||
|
||||
z_offset = max_z - current_z + Z_FINAL_OFFSET
|
||||
|
||||
robot_z.move(max_z + Z_FINAL_OFFSET)
|
||||
|
||||
print "Z offset = ", z_offset
|
||||
|
||||
|
||||
#Updating tool:
|
||||
#update_tool(None, x_offset=x_offset, z_offset=z_offset)
|
||||
@@ -0,0 +1,60 @@
|
||||
import plotutils
|
||||
from mathutils import fit_gaussian, Gaussian
|
||||
|
||||
|
||||
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/ScanYZ")
|
||||
|
||||
robot.set_motors_enabled(True)
|
||||
|
||||
|
||||
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 YZ 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/ScanYZ")
|
||||
|
||||
robot.set_motors_enabled(True)
|
||||
|
||||
second_y = robot_y.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
|
||||
t=robot.get_tool_trsf(TOOL_DEFAULT)
|
||||
t[0]=xoff
|
||||
t[1]=-yoff
|
||||
robot.set_tool_trsf(t, TOOL_DEFAULT)
|
||||
|
||||
|
||||
|
||||
@@ -0,0 +1,86 @@
|
||||
import plotutils
|
||||
from mathutils import fit_gaussian, Gaussian
|
||||
|
||||
|
||||
#robot.assert_tool(TOOL_CALIBRATION)
|
||||
#cal_tool = TOOL_DEFAULT
|
||||
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.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", FRAME_TABLE, "tcp_t" )
|
||||
robot.set_pnt(c, "pTemp")
|
||||
robot.movel("pTemp", cal_tool, 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
|
||||
print "Offset: ", [t[0], t[1]]
|
||||
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"
|
||||
else:
|
||||
print "Cannot move calibrated tool to laser: toog big offset"
|
||||
@@ -0,0 +1,83 @@
|
||||
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()
|
||||
try:
|
||||
robot.set_frame(FRAME_TABLE)
|
||||
run("calibration/ScanX")
|
||||
finally:
|
||||
robot.set_default_frame()
|
||||
|
||||
pos1 = robot.get_cartesian_pos()
|
||||
x1, l1 = closest_x, closest_y
|
||||
|
||||
print "Scan 1 result: ", [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)
|
||||
|
||||
|
||||
try:
|
||||
robot.set_frame(FRAME_TABLE)
|
||||
run("calibration/ScanX")
|
||||
finally:
|
||||
robot.set_default_frame()
|
||||
|
||||
pos2 =robot.get_cartesian_pos()
|
||||
x2, l2 = closest_x, closest_y
|
||||
|
||||
print "Scan 2 result: ", [x2, l2]
|
||||
print "Position 2: ", pos1
|
||||
|
||||
off_l = l2 - l1
|
||||
print "Offset l: ", off_l
|
||||
|
||||
#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([0, -off_l, 0, 0, 0, 0])
|
||||
c=robot.compose("pTemp", FRAME_TABLE, "tcp_t" )
|
||||
robot.set_pnt(c, "pTemp")
|
||||
robot.movel("pTemp", cal_tool, DESC_SCAN, sync=True)
|
||||
|
||||
|
||||
pos3 = robot.get_cartesian_pos()
|
||||
print "Position 3: ", pos3
|
||||
|
||||
|
||||
#Updates the tool
|
||||
t=robot.get_tool_trsf(TOOL_DEFAULT)
|
||||
print "Former tool: " + str(t)
|
||||
|
||||
xoff = (pos3[0]-pos1[0])/2
|
||||
yoff = (pos3[1]-pos1[1])/2
|
||||
xrot = math.degrees(math.atan(yoff/t[2]))
|
||||
yrot = math.degrees(math.atan(xoff/t[2]))
|
||||
t[0]=xoff
|
||||
t[1]=-yoff
|
||||
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"
|
||||
@@ -0,0 +1,79 @@
|
||||
import plotutils
|
||||
from mathutils import fit_gaussian, Gaussian
|
||||
|
||||
cal_tool = TOOL_CALIBRATION
|
||||
|
||||
robot.set_tool(cal_tool)
|
||||
|
||||
robot.enable()
|
||||
move_to_laser("pPark")
|
||||
|
||||
|
||||
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 "Scan 1 result: ", [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")
|
||||
|
||||
|
||||
pos2 =robot.get_cartesian_pos()
|
||||
x2, l2 = closest_x, closest_y
|
||||
|
||||
print "Scan 2 result: ", [x2, l2]
|
||||
print "Position 2: ", pos1
|
||||
|
||||
off_l = l2 - l1
|
||||
print "Offset l: ", off_l
|
||||
|
||||
|
||||
#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([0, -off_l, 0, 0, 0, 0])
|
||||
c=robot.compose("pTemp", FRAME_DEFAULT, "tcp_t" )
|
||||
robot.set_pnt(c, "pTemp")
|
||||
robot.movel("pTemp", cal_tool, DESC_SCAN, sync=True)
|
||||
|
||||
pos3 = robot.get_cartesian_pos()
|
||||
print "Position 3: ", pos3
|
||||
|
||||
|
||||
#Updates the tool
|
||||
t=robot.get_tool_trsf(TOOL_DEFAULT)
|
||||
print "Former tool: " + str(t)
|
||||
|
||||
xoff = (pos3[0]-pos1[0])/2
|
||||
yoff = (pos3[1]-pos1[1])/2
|
||||
xrot = math.degrees(math.atan(yoff/t[2]))
|
||||
yrot = math.degrees(math.atan(xoff/t[2]))
|
||||
t[0]=xoff
|
||||
t[1]=-yoff
|
||||
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"
|
||||
|
||||
|
||||
|
||||
@@ -0,0 +1,79 @@
|
||||
import plotutils
|
||||
from mathutils import fit_gaussian, Gaussian
|
||||
|
||||
cal_tool = TOOL_CALIBRATION
|
||||
|
||||
robot.set_tool(cal_tool)
|
||||
|
||||
robot.enable()
|
||||
move_to_laser("pPark")
|
||||
|
||||
|
||||
robot.set_motors_enabled(True)
|
||||
robot.set_joint_motors_enabled(True)
|
||||
initial_pos = robot.get_cartesian_pos()
|
||||
|
||||
#robot.align()
|
||||
run("calibration/ScanY")
|
||||
|
||||
pos1 = robot.get_cartesian_pos()
|
||||
l1, y1 = closest_x, closest_y
|
||||
|
||||
print "Scan 1 result: ", [l1, 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")
|
||||
|
||||
pos2 =robot.get_cartesian_pos()
|
||||
l2, y2 = closest_x, closest_y
|
||||
|
||||
print "Scan 2 result: ", [l2, y2]
|
||||
print "Position 2: ", pos1
|
||||
|
||||
off_l = l2 - l1
|
||||
print "Offset l: ", off_l
|
||||
|
||||
#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_l, 0, 0, 0, 0, 0])
|
||||
c=robot.compose("pTemp", FRAME_DEFAULT, "tcp_t" )
|
||||
robot.set_pnt(c, "pTemp")
|
||||
robot.movel("pTemp", cal_tool, DESC_SCAN, sync=True)
|
||||
|
||||
|
||||
pos3 = robot.get_cartesian_pos()
|
||||
print "Position 3: ", pos3
|
||||
|
||||
|
||||
#Updates the tool
|
||||
t=robot.get_tool_trsf(TOOL_DEFAULT)
|
||||
print "Former tool: " + str(t)
|
||||
|
||||
xoff = (pos3[0]-pos1[0])/2
|
||||
yoff = (pos3[1]-pos1[1])/2
|
||||
xrot = math.degrees(math.atan(yoff/t[2]))
|
||||
yrot = math.degrees(math.atan(xoff/t[2]))
|
||||
t[0]=xoff #X
|
||||
t[1]=-yoff #Y
|
||||
#t[3]= xropt #RX
|
||||
#t[4]= yropt #RY
|
||||
|
||||
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"
|
||||
Reference in New Issue
Block a user