From dd1359858b355f0296ef247136a755fd08e48223 Mon Sep 17 00:00:00 2001 From: gac-S_Changer Date: Wed, 6 Dec 2017 15:16:07 +0100 Subject: [PATCH] --- config/devices.properties | 4 +- script/calibration/ScanXZ.py | 28 ++++--- script/calibration/ToolCalibration.py | 64 +++++++++------ script/devices/RobotMotors.py | 21 ++--- script/devices/RobotSC.py | 16 ++-- script/devices/RobotTCP.py | 111 ++++++++++++++++++-------- script/motion/tools.py | 9 +-- script/test/TestAlign.py | 9 +++ 8 files changed, 161 insertions(+), 101 deletions(-) create mode 100644 script/test/TestAlign.py diff --git a/config/devices.properties b/config/devices.properties index 6a06931..58cd10e 100644 --- a/config/devices.properties +++ b/config/devices.properties @@ -1,10 +1,10 @@ -img=ch.psi.pshell.prosilica.Prosilica|25001 ''||-100|false +img=ch.psi.pshell.prosilica.Prosilica|25001 ""||-100|false microscan=ch.psi.pshell.serial.TcpDevice|129.129.126.200:2001||| microscan_cmd=ch.psi.pshell.serial.TcpDevice|129.129.126.200:2003||| ue=LaserUE|COM4||| #robot=RobotTcp|127.0.0.1:1000||| #onewire=ch.psi.pshell.serial.TcpDevice|129.129.126.83:5000||| -puck_detection=ch.psi.mxsc.PuckDetection|raspberrypi:5556||| +#puck_detection=ch.psi.mxsc.PuckDetection|raspberrypi:5556||| #robot_modbus=ch.psi.pshell.modbus.ModbusTCP|129.129.126.100:502||| #jf1=ch.psi.pshell.modbus.AnalogInput|robot_modbus 0||100| #jf2=ch.psi.pshell.modbus.AnalogInput|robot_modbus 1||100| diff --git a/script/calibration/ScanXZ.py b/script/calibration/ScanXZ.py index 96cbcd6..8bc3b26 100644 --- a/script/calibration/ScanXZ.py +++ b/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 diff --git a/script/calibration/ToolCalibration.py b/script/calibration/ToolCalibration.py index 26e2cac..9b7fb31 100644 --- a/script/calibration/ToolCalibration.py +++ b/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) - diff --git a/script/devices/RobotMotors.py b/script/devices/RobotMotors.py index 2b0ad33..623da7f 100644 --- a/script/devices/RobotMotors.py +++ b/script/devices/RobotMotors.py @@ -14,7 +14,6 @@ class RobotCartesianMotor (PositionerBase): #ATTENTION: Always initialize cartesian motors before scanning (or call robot.set_motors_enabled(True)) def doInitialize(self): - self.setCache(None, None) self.setCache(self.doRead(), None) def doRead(self): @@ -23,16 +22,11 @@ class RobotCartesianMotor (PositionerBase): def doWrite(self, value): if self.robot.cartesian_destination is not None: print "Move " + ROBOT_MOTORS[self.index] + " to " + str(value) - self.robot.updating = True - try: - robot.cartesian_destination[self.index] = float(value) - self.robot.set_pnt(robot.cartesian_destination , "tcp_p") - self.robot.movel("tcp_p", self.robot.tool , DESC_SCAN) - except: - self.robot.updating = False + robot.cartesian_destination[self.index] = float(value) + self.robot.set_pnt(robot.cartesian_destination , "tcp_p") + self.robot.movel("tcp_p", self.robot.tool , DESC_SCAN) def doReadReadback(self): - #return float(self.robot.get_cartesian_pos(self._get_tool(),self.frame)[self.index]) return float("nan") if self.robot.cartesian_pos is None else float(self.robot.cartesian_pos[self.index]) @@ -59,13 +53,8 @@ class RobotJointMotor (PositionerBase): self.setpoint = value joint = self.robot.herej() joint[self.index] = value - self.robot.updating = True - try: - self.robot.set_jnt(joint, name="tcp_j") - self.robot.movej("tcp_j", self.robot.tool , DESC_SCAN) - finally: - self.robot.updating = False - + self.robot.set_jnt(joint, name="tcp_j") + self.robot.movej("tcp_j", self.robot.tool , DESC_SCAN) def doReadReadback(self): return self.robot.herej()[self.index] if self.robot.joint_pos is None else float(self.robot.joint_pos[self.index]) diff --git a/script/devices/RobotSC.py b/script/devices/RobotSC.py index c713e72..da3648e 100644 --- a/script/devices/RobotSC.py +++ b/script/devices/RobotSC.py @@ -11,14 +11,13 @@ DESC_DEFAULT = DESC_FAST run("devices/RobotTCP") -simulation = False +simulation = True joint_forces = False class RobotSC(RobotTCP): def __init__(self, name, server, timeout = 1000, retries = 1): - RobotTCP.__init__(self, name, server, timeout, retries) - self.tool = TOOL_CALIBRATION + RobotTCP.__init__(self, name, server, timeout, retries) def mount(self, puck, sample): return self.execute('mount',segment, puck, sample) @@ -58,14 +57,7 @@ class RobotSC(RobotTCP): def set_local(self): robot.set_profile("default") - - def set_tool(self,tool): - if tool != self.tool: - self.tool = tool - for dev in (robot_x, robot_y, robot_z, robot_rx, robot_ry, robot_rz): - dev.initialize() - dev.update() - + if simulation: @@ -75,7 +67,9 @@ else: add_device(RobotSC("robot", "129.129.110.100:1000"), force = True) robot.high_level_tasks = ["mount", "firstmount"] +robot.set_tool(TOOL_CALIBRATION) robot.setPolling(500) + #robot.set_monitor_speed(20) diff --git a/script/devices/RobotTCP.py b/script/devices/RobotTCP.py index 4738ab8..89a91c8 100644 --- a/script/devices/RobotTCP.py +++ b/script/devices/RobotTCP.py @@ -1,6 +1,7 @@ import threading FRAME_DEFAULT = "world" +FLANGE = "flange" run("devices/RobotMotors") @@ -27,15 +28,16 @@ class RobotTCP(TcpDevice, Stoppable): self.current_task = None self.high_level_tasks = [] self.cartesian_destination = None - self.cartesian_pos = None - self.joint_pos = None + #self.flange_pos = [None] * 6 + self.cartesian_pos = [None] * 6 + self.joint_pos = [None] * 6 self.cartesian_motors_enabled = False self.cartesian_motors = [] self.joint_motors_enabled = False self.joint_motors = [] self.tool = None + #self.tool_trsf = [0.0] * 6 self.frame = FRAME_DEFAULT - self.updating = False def doInitialize(self): @@ -45,8 +47,9 @@ class RobotTCP(TcpDevice, Stoppable): def set_tool(self,tool): self.tool = tool - if self.cartesian_motors_enabled: - self.cartesian_pos = self.get_cartesian_pos() + #self.tool_trsf = self.get_tool_trsf() + if self.cartesian_motors_enabled: + self.update() for m in self.cartesian_motors: m.initialize() m.update() @@ -54,7 +57,10 @@ class RobotTCP(TcpDevice, Stoppable): def get_tool(self): return self.tool - + + def assert_tool(self, tool): + if self.tool != tool: + raise Exception("Invalid tool: " + self.tool) def _sendReceive(self, msg_id, msg = "", timeout = None): tx = self.header if (self.header != None) else "" @@ -313,25 +319,44 @@ class RobotTCP(TcpDevice, Stoppable): self.joint_forces = None raise - def movej(self, joint_or_point, tool, desc): - return self.eval_int("movej(" + joint_or_point + ", " + tool + ", " + desc +")") + def movej(self, joint_or_point, tool, desc, sync=False): + ret = self.eval_int("movej(" + joint_or_point + ", " + tool + ", " + desc +")") + if sync: + self.wait_end_of_move() + return ret - def movel(self, point, tool, desc): - return self.eval_int("movel(" + point + ", " + tool + ", " + desc +")") + def movel(self, point, tool, desc, sync=False): + ret = self.eval_int("movel(" + point + ", " + tool + ", " + desc +")") + if sync: + self.wait_end_of_move() + return ret - def movec(self, point_interm, point_target, tool, desc): - return self.eval_int("movec(" + point_interm + ", " + point_target + ", " + tool + ", " + desc +")") + def movec(self, point_interm, point_target, tool, desc, sync=False): + ret = self.eval_int("movec(" + point_interm + ", " + point_target + ", " + tool + ", " + desc +")") + if sync: + self.wait_end_of_move() + return ret + def wait_end_of_move(self): + time.sleep(0.05) + self.update() + #time.sleep(0.01) + #robot.waitCacheChange(-1) + self.waitReady(-1) + #robot.waitState(State.Ready, -1) + #Tool #This function can timeout since it synchronizes move. #Better check state before otherwise it can freeze the communication - def open_tool(self, tool): + def open_tool(self, tool=None): + if tool is None: tool = self.tool return self.evaluate("open(" + tool + " )", timeout=3000) #This function can timeout since it synchronizes move. Better check state before #Better check state before otherwise it can freeze the communication - def close_tool(self, tool): + def close_tool(self, tool=None): + if tool is None: tool = self.tool return self.evaluate("close(" + tool + " )", timeout=3000) #Arm position @@ -424,19 +449,30 @@ class RobotTCP(TcpDevice, Stoppable): try: start = time.time() #sts = self._sendReceive("EVT").strip().split(self.array_separator) - sts = self.execute("get_status", self.current_task) + sts = self.execute("get_status", self.current_task, self.tool, self.frame) self._update_working_mode(int(sts[0]), int(sts[1])) self.powered = sts[2] == "1" self.speed = int(sts[3]) self.empty = sts[4] == "1" self.settled = sts[5] == "1" - ev = sts[7] if len(sts)>7 else "" + if int(sts[6]) < 0: + self.current_task = None + + for i in range(6): + self.joint_pos[i] = float(sts[7 + i]) + for i in range(6): + #self.flange_pos[i] = float(sts[13 + i]) + #self.tool_pos[i] = (self.flange_pos[i] - self.tool_trsf[i] ) if i<3 else (self.flange_pos[i] + self.tool_trsf[i] ) + self.cartesian_pos[i] = float(sts[13 + i]) + + ev_index = 19 #7 + ev = sts[ev_index] if len(sts)>ev_index else "" if len(ev.strip()) >0: self.getLogger().info(ev) self.on_event(ev) + + #if (self.current_task is not None) and (self.get_task_status(self.current_task)[0]<0): - if int(sts[6]) < 0: - self.current_task = None self._update_state() self.reset = False self.setCache({"powered": self.powered, @@ -447,24 +483,22 @@ class RobotTCP(TcpDevice, Stoppable): "mode": self.working_mode, "status": self.status }, None) - if not self.updating: - if self.cartesian_motors_enabled: - self.cartesian_pos = self.get_cartesian_pos() - for m in self.cartesian_motors: - #m.update() - m.readback.update() - else: - self.cartesian_pos = None - if self.joint_motors_enabled: - self.joint_pos = self.herej() - for m in self.joint_motors: - m.readback.update() + if self.cartesian_motors_enabled: + for m in self.cartesian_motors: + m.readback.update() + + if self.joint_motors_enabled: + for m in self.joint_motors: + m.readback.update() + except: if self.state != State.Offline: print >> sys.stderr, "Update error: " + str(sys.exc_info()[1]) - self.setState(State.Offline) + self.setState(State.Offline) #Cartesian space + def get_flange_pos(self): + return self.eval_pnt("jointToPoint(" +FLANGE + ", " + self.frame + ", herej())") def get_cartesian_pos(self): return self.eval_pnt("jointToPoint(" + self.tool + ", " + self.frame + ", herej())") @@ -512,14 +546,23 @@ class RobotTCP(TcpDevice, Stoppable): for m in self.joint_motors: remove_device(m) self.joint_motors = [] - self.joint_destination = None else: if value: - self.joint_destination = self.get_joint_destination() for m in self.joint_motors: m.initialize() + #Alignment + def align(self, point = None, frame = FRAME_DEFAULT, desc = DESC_FAST): + if point is None: + self.get_cartesian_pos() + else: + self.set_pnt(point) + np=self.eval_trf('align(tcp_p.trsf, ' + FRAME_DEFAULT + '.trsf)') + self.set_pnt(np) + self.movej("tcp_p", self.tool, desc) + #TODO: The first command is not executed (but receive a move id) + self.movej("tcp_p", self.tool, desc, sync = True) + return np - #High-level, exclusive motion task. def start_task(self, program, *args, **kwargs): tasks = [t for t in self.high_level_tasks] diff --git a/script/motion/tools.py b/script/motion/tools.py index 7706876..08d2f05 100644 --- a/script/motion/tools.py +++ b/script/motion/tools.py @@ -19,8 +19,7 @@ def move_to_laser(): if d