76 lines
1.3 KiB
Python
76 lines
1.3 KiB
Python
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)
|
|
|