61 lines
1.1 KiB
Python
61 lines
1.1 KiB
Python
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/ScanXZ")
|
|
|
|
robot.set_motors_enabled(True)
|
|
|
|
|
|
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)
|
|
|
|
|
|
|