diff --git a/script/devices/RobotBernina.py b/script/devices/RobotBernina.py index 25d29d9..fb60943 100644 --- a/script/devices/RobotBernina.py +++ b/script/devices/RobotBernina.py @@ -17,6 +17,10 @@ DEFAULT_ROBOT_POLLING = 200 TASK_WAIT_ROBOT_POLLING = 50 DEFAULT_SPEED=100 +ROBOT_JOINT_MOTORS = ["j1" , "j2", "j3", "j4", "j5", "j6"] +ROBOT_CARTESIAN_MOTORS = ["x" , "y", "z", "rx", "ry", "rz"] +ROBOT_SPHERICAL_MOTORS = ["r" , "gamma", "delta"] + run("devices/RobotTCP") @@ -32,6 +36,19 @@ class RobotBernina(RobotTCP): self.last_command_timestamp = None self.last_command_position = None #self.setSimulated() # TODO: Remove me + + def set_motion_queue_empty(self, state=False): + ### update the status of the robot, the queue of motion commands + ### the status is polled automatically from the controller but this setting command does not have a delay + self.empty = state + self._update_state() + + def check_software_limits(self, j1=0, j2=0, j3=0, j4=0, j5=0, j6=0): + j = [j1,j2,j3,j4,j5,j6] + self.set_jnt(j, name="tcp_j") + self.evaluate("tcp_b=isInRange(tcp_j)") + return self.get_bool(name="tcp_b") + def deg2rad(self, angles): return [a/180.*math.pi for a in angles] @@ -46,7 +63,6 @@ class RobotBernina(RobotTCP): def update_spherical_pos(self): self.spherical_pos=self.cart2sph(return_dict=True, **self.cartesian_pos) - def get_movel_interpolation(self, i, coordinates="joint", idx_pnt1=0, idx_pnt2=1): """ coordinates: "joint" or "cartesian" @@ -155,6 +171,7 @@ class RobotBernina(RobotTCP): for i in range(11): sim.append(self.get_movel_interpolation(i/10., coordinates=coordinates)) else: + self.set_motion_queue_empty(False) ret = self.movel("tcp_p_spherical[1]", tool=tool, desc=None, sync=sync) if simulate: return sim @@ -207,6 +224,7 @@ class RobotBernina(RobotTCP): for i in range(11): sim.append(self.get_movel_interpolation(i/10., coordinates=coordinates)) else: + self.set_motion_queue_empty(False) ret = self.movel("tcp_p_spherical[1]", tool=tool, desc=None, sync=sync) print("moving radius") @@ -227,21 +245,45 @@ class RobotBernina(RobotTCP): if (self.point_reachable("tcp_p_spherical[2]"))&(self.point_reachable("tcp_p_spherical[3]"))&(self.distance_p("tcp_p_spherical[1]", "tcp_p_spherical[3]")>.1): if self.distance_p("tcp_p_spherical[2]", "tcp_p_spherical[3]")>1: if simulate: - for i in range(10): - sim.append(self.get_movec_interpolation(i/10.+0.1, coordinates=coordinates)) + if len(sim)==0: + for i in range(11): + sim.append(self.get_movec_interpolation(i/10., coordinates=coordinates)) + else: + for i in range(10): + sim.append(self.get_movec_interpolation(i/10.+0.1, coordinates=coordinates)) else: ret = self.movec("tcp_p_spherical[2]", "tcp_p_spherical[3]", tool=None, desc=None, sync=sync) print("moving angle circle") else: if simulate: - for i in range(10): - sim.append(self.get_movel_interpolation(i/10.+0.1, coordinates=coordinates, idx_pnt1=1, idx_pnt2=3)) + if len(sim==0): + for i in range(11): + sim.append(self.get_movel_interpolation(i/10., coordinates=coordinates, idx_pnt1=1, idx_pnt2=3)) + else: + for i in range(10): + sim.append(self.get_movel_interpolation(i/10.+0.1, coordinates=coordinates, idx_pnt1=1, idx_pnt2=3)) else: + self.set_motion_queue_empty(False) ret = self.movel("tcp_p_spherical[3]", tool=None, desc=None, sync=sync) print("moving angle linear") if simulate: return sim - + + def move_joint(self, j1=None, j2=None, j3=None, j4=None, j5=None, j6=None, tool=None, desc=None, sync=False, simulate=False, coordinates="joint"): + # Simulation not implemented yet + if desc == None: + desc = self.default_desc + if tool == None: + tool = self.tool + target_jnt = {"j1": j1, "j2": j2, "j3": j3, "j4": j4, "j5": j5, "j6": j6} + current_jnt = self.get_joint_pos(return_dict=True) + for k, v in target_jnt.items(): + if v == None: + target_jnt[k] = current_jnt[k] + self.robot.set_jnt(target_jnt, name="tcp_j") + self.set_motion_queue_empty(False) + self.robot.movej("tcp_j", tool , desc) + def move_home(self): if not self.is_in_point(P_HOME): self.start_task(MOVE_HOME) @@ -348,5 +390,5 @@ robot.setPolling(DEFAULT_ROBOT_POLLING) robot.get_current_point() #TODO: REMOVE WHEN CURRENT POINT REPORTED BY POLLING MESSAGE robot.set_motors_enabled(True) robot.set_joint_motors_enabled(True) -robot.motor_to_epics_pvs(robot.cartesian_motors+robot.spherical_motors+robot.joint_motors) +robot.motor_to_epics_pvs(robot.cartesian_motors+robot.spherical_motors+robot.joint_motors+robot.linear_axis_motors) #show_panel(robot) diff --git a/script/devices/RobotMotors.py b/script/devices/RobotMotors.py index 1f1a9ac..06b4e39 100644 --- a/script/devices/RobotMotors.py +++ b/script/devices/RobotMotors.py @@ -6,8 +6,13 @@ class LinearAxisMotor(PositionerBase): PositionerBase.__init__(self, name, PositionerConfig()) self.robot = robot + @property + def target_coordinates(self): + return {k: m.setpoint for k,m in robot.linear_axis_motors.items()} + #ATTENTION: Always initialize linear axis motors before scanning (or call robot.set_lin_axis_motor_enabled(True)) def doInitialize(self): + self.setpoint = self.doReadReadback() self.setCache(self.doRead(), None) def doRead(self): @@ -15,29 +20,32 @@ class LinearAxisMotor(PositionerBase): def doWrite(self, value): if self.robot.linear_axis_destination is not None: + self.setpoint = value self.robot.linear_axis_destination.update({self.name: value}) self.setCache(float(value), None) robot.evaluate("n_moveAbsPos = " + str(value)) robot.evaluate("setEcAo(ecao_posSet,n_moveAbsPos)") def doReadReadback(self): - return float("nan") if self.robot.linear_axis_pos is None else float(self.robot.linear_axis_pos[self.name]) + return float("nan") if len(self.robot.linear_axis_pos)==0 else float(self.robot.linear_axis_pos[self.name]) def stop(self): robot.evaluate("setEcDo(ecdo_linMotStop,true)") time.sleep(0.1) robot.evaluate("setEcDo(ecdo_linMotStop,false)") - -ROBOT_MOTORS = ["x" , "y", "z", "rx", "ry", "rz"] class RobotCartesianMotor(PositionerBase): - def __init__(self, robot, index): - PositionerBase.__init__(self, ROBOT_MOTORS[index], PositionerConfig()) - self.index = index + def __init__(self, robot, name): + PositionerBase.__init__(self, name, PositionerConfig()) self.robot = robot + + @property + def target_coordinates(self): + return {k: m.setpoint for k,m in robot.cartesian_motors.items()} #ATTENTION: Always initialize cartesian motors before scanning (or call robot.set_motors_enabled(True)) def doInitialize(self): + self.setpoint = self.doReadReadback() self.setCache(self.doRead(), None) def doRead(self): @@ -45,11 +53,12 @@ class RobotCartesianMotor(PositionerBase): def doWrite(self, value): if self.robot.cartesian_destination is not None: + self.setpoint = value self.setCache(float(value), None) robot.move_cartesian(**{self.name:value}) def doReadReadback(self): - return float("nan") if self.robot.cartesian_pos is None else float(self.robot.cartesian_pos[self.name]) + return float("nan") if len(self.robot.cartesian_pos)==0 else float(self.robot.cartesian_pos[self.name]) def stop(self): robot.stop() @@ -59,40 +68,46 @@ class RobotCartesianMotor(PositionerBase): def move_done(self): """once move done, do readback update and send event and readback""" -ROBOT_SPHERICAL_MOTORS = ["r" , "gamma", "delta"] - class RobotSphericalMotor (PositionerBase): - def __init__(self, robot, index): - PositionerBase.__init__(self, ROBOT_SPHERICAL_MOTORS[index], PositionerConfig()) - self.index = index + def __init__(self, robot, name): + PositionerBase.__init__(self, name, PositionerConfig()) self.robot = robot - + + @property + def target_coordinates(self): + return {k: m.setpoint for k,m in robot.spherical_motors.items()} + def doInitialize(self): + self.setpoint = self.doReadReadback() self.setCache(self.doRead(), None) def doRead(self): - return float("nan") if self.robot.spherical_pos is None else robot.spherical_pos[self.name] + return float("nan") if len(self.robot.spherical_pos)==0 else robot.spherical_pos[self.name] def doWrite(self, value): if self.robot.spherical_destination is not None: + self.setpoint = value self.setCache(float(value), None) robot.move_spherical(**{self.name:value}) def doReadReadback(self): - return float("nan") if self.robot.spherical_pos is None else robot.spherical_pos[self.name] + return float("nan") if len(self.robot.spherical_pos)==0 else robot.spherical_pos[self.name] def stop(self): robot.stop() robot.reset_motion() robot.resume() -ROBOT_JOINT_MOTORS = ["j1" , "j2", "j3", "j4", "j5", "j6"] + class RobotJointMotor (PositionerBase): - def __init__(self, robot, index): - PositionerBase.__init__(self, ROBOT_JOINT_MOTORS[index], PositionerConfig()) - self.index = index - self.robot = robot - + def __init__(self, robot, name): + PositionerBase.__init__(self, name, PositionerConfig()) + self.robot = robot + + @property + def target_coordinates(self): + return {k: m.setpoint for k,m in robot.joint_motors.items()} + def doInitialize(self): self.setpoint = self.doReadReadback() self.setCache(self.doRead(), None) @@ -100,16 +115,13 @@ class RobotJointMotor (PositionerBase): def doRead(self): return self.setpoint - def doWrite(self, value): - #print "Move " + ROBOT_JOINT_MOTORS[self.index] + " to " + str(value) + def doWrite(self, value): self.setpoint = value - joint = self.robot.herej() - joint[self.index] = value - self.robot.set_jnt(joint, name="tcp_j") - self.robot.movej("tcp_j", self.robot.tool , DESC_DEFAULT) - + self.robot.move_joint(**{self.name:value}) + + def doReadReadback(self): - return self.robot.herej()[self.index] if self.robot.joint_pos is None else float(self.robot.joint_pos[self.name]) + return self.robot.get_joint_pos()[self.name] if len(self.robot.joint_pos)==0 else float(self.robot.joint_pos[self.name]) def stop(self): robot.stop() diff --git a/script/devices/RobotTCP.py b/script/devices/RobotTCP.py index 374d6bd..cb382d9 100644 --- a/script/devices/RobotTCP.py +++ b/script/devices/RobotTCP.py @@ -39,13 +39,13 @@ class RobotTCP(TcpDevice, Stoppable): self.joint_pos = {} self.linear_axis_pos = {} self.cartesian_motors_enabled = False - self.cartesian_motors = [] + self.cartesian_motors = {} self.spherical_motors_enabled = False - self.spherical_motors = [] + self.spherical_motors = {} self.linear_axis_motors_enabled = None - self.linear_axis_motors = [] + self.linear_axis_motors = {} self.joint_motors_enabled = False - self.joint_motors = [] + self.joint_motors = {} self.tool = None self.default_desc = None self.tool_open = None @@ -354,19 +354,50 @@ class RobotTCP(TcpDevice, Stoppable): if str(ret) != "0": raise Exception("Error saving program: " + str(ret)) def _update_working_mode(self, mode, status): + modestr = { + 1: { + "mode": "manual", + "status": { + 0: "programmed", + 1: "connection", + 2: "joint", + 3: "cartesian_frame", + 4: "cartesian_tool", + 5: "point", + 6: "hold", + }, + }, + 2: { + "mode": "test", + "status": { + 0: "programmed", + 1: "connection", + 2: "programmed_fast", + 3: "hold", + }, + }, + 3: { + "mode": "local", + "status": { + 0: "programmed", + 1: "connection", + 2: "hold", + }, + }, + 4: { + "mode": "remote", + "status": { + 0: "programmed", + 1: "connection", + 2: "hold", + }, + }, + } + cur_mode = self.working_mode - if mode==1: - self.working_mode = "manual" - self.status = "hold" if status==6 else "move" - elif mode==2: - self.working_mode = "test" - self.status = "hold" if status==3 else "move" - elif mode==3: - self.working_mode = "local" - self.status = "hold" if status==2 else "move" - elif mode==4: - self.working_mode = "remote" - self.status = "hold" if status==2 else "move" + if mode in modestr.keys(): + self.working_mode = modestr[mode]["mode"] + self.status = modestr[mode]["status"][status] else: self.working_mode = "invalid" self.status = "invalid" @@ -407,12 +438,20 @@ class RobotTCP(TcpDevice, Stoppable): self.evaluate("restartMove()") def reset_motion(self, joint=None, timeout=None): - #TODO: in new robot robot.resetMotion() is freezing controller self.evaluate("resetMotion()" if (joint is None) else ("resetMotion(" + joint + ")")) - #if joint is None: - # self.execute('reset', timeout=timeout) - #else: - # self.execute('reset', str(joint), timeout=timeout) + ### reinitialize all motors (target values) + if self.cartesian_motors_enabled: + for m in self.cartesian_motors.values(): + m.doInitialize() + if self.spherical_motors_enabled: + for m in self.spherical_motors.values(): + m.doInitialize() + if self.linear_axis_motors_enabled: + for m in self.linear_axis_motors.values(): + m.doInitialize() + if self.joint_motors_enabled: + for m in self.joint_motors.values(): + m.doInitialize() def is_empty(self): self.empty = self.eval_bool("isEmpty()") @@ -742,12 +781,12 @@ class RobotTCP(TcpDevice, Stoppable): else: sts = self.execute("get_status_fast", cur_task) - for i in range(6): - self.joint_pos[ROBOT_JOINT_MOTORS[i]] = float(sts[0 + i]) - for i in range(6): - self.cartesian_pos[ROBOT_MOTORS[i]] = float(sts[6 + i]) + for i, name in enumerate(ROBOT_JOINT_MOTORS): + self.joint_pos[name] = float(sts[0 + i]) + for i, name in enumerate(ROBOT_CARTESIAN_MOTORS): + self.cartesian_pos[name] = float(sts[6 + i]) - self.linear_axis_pos = {"z_lin": float(sts[7])} + self.linear_axis_pos = {"z_lin": float(sts[12])} self.update_spherical_pos() ev_index = 8 #7 @@ -767,19 +806,20 @@ class RobotTCP(TcpDevice, Stoppable): self.on_poll_status.update({ "pos": pos, }) - + if self.cartesian_motors_enabled: - for m in self.cartesian_motors: + for m in self.cartesian_motors.values(): m.readback.update() if self.spherical_motors_enabled: - for m in self.spherical_motors: + for m in self.spherical_motors.values(): m.readback.update() if self.linear_axis_motors_enabled: - for m in self.linear_axis_motors: + for m in self.linear_axis_motors.values(): m.readback.update() if self.joint_motors_enabled: - for m in self.joint_motors: + for m in self.joint_motors.values(): m.readback.update() + def doUpdate(self): try: @@ -884,16 +924,16 @@ class RobotTCP(TcpDevice, Stoppable): get_context().sendEvent("polling", self.on_poll_status) if self.cartesian_motors_enabled: - for m in self.cartesian_motors: + for m in self.cartesian_motors.values(): m.readback.update() if self.spherical_motors_enabled: - for m in self.spherical_motors: + for m in self.spherical_motors.values(): m.readback.update() if self.linear_axis_motors_enabled: - for m in self.linear_axis_motors: + for m in self.linear_axis_motors.values(): m.readback.update() if self.joint_motors_enabled: - for m in self.joint_motors: + for m in self.joint_motors.values(): m.readback.update() @@ -908,27 +948,30 @@ class RobotTCP(TcpDevice, Stoppable): "powered": {"cmd":"robot.set_powered", "def_kwargs": ""}, "speed": {"cmd":"robot.set_monitor_speed", "def_kwargs": ""}, "frame": {"cmd": "robot.set_frame", "def_kwargs": "change_default=True",}, - "frame_coordinates": {"cmd": "robot.set_trsf", "def_kwargs": "name='f_actualFrame.trsf'",}, + "frame_coordinates": {"cmd": "robot.set_frame_coordinates", "def_kwargs": "",}, "tool": {"cmd":"robot.set_tool", "def_kwargs": ""}, - "tool_coordinates": {"cmd": "robot.set_trsf", "def_kwargs": "name='t_actualTool.trsf'",}, + "tool_coordinates": {"cmd": "robot.set_tool_coordinates", "def_kwargs": "",}, "cartesian_motors_enabled": {"cmd":"robot.set_motors_enabled", "def_kwargs": "pseudos=['cartesian']"}, "spherical_motors_enabled": {"cmd":"robot.set_motors_enabled", "def_kwargs": "pseudos=['spherical']"}, } on_poll_info = [k for k in self.on_poll_status if not k in list(on_poll_config_methods.keys())+["pos"]] return {"info": on_poll_info, "config": on_poll_config_methods} + def set_tool_coordinates(self, v, name=None): + if name is None: + name = self.tool + self.set_trsf(v, name=name + ".trsf") + if name == self.tool: + self.set_trsf(v, name="t_actualTool.trsf") + + def set_frame_coordinates(self, v, name=None): + if name is None: + name = self.frame + self.set_trsf(v, name=name + ".trsf") + if name == self.frame: + self.set_trsf(v, name="f_actualFrame.trsf") + #Cartesian space - """ - def get_cartesian_pos(self): - self.assert_tool() - return self.eval_pnt("jointToPoint(" + self.tool + ", " + self.frame + ", herej())") - #self.set_jnt(self.herej()) - #return self.joint_to_point(tool, frame) - """ - - - #TODO: in new robot robot.evaluate("tcp_p=jointToPoint(tSuna, world, herej())") is freezing controller (cannot call herej directly) - #We can consider atomic because tcp_j is only accessed in comTcp def get_cartesian_pos(self, tool=None, frame=None, return_dict=True): if tool is None: self.assert_tool() @@ -945,11 +988,14 @@ class RobotTCP(TcpDevice, Stoppable): else: ret = [x, y, z, rx, ry, rz] return ret - #a = self.execute('get_pos', tool, frame) - #ret = [] - #for i in range(6): ret.append(float(a[i])) - #return ret + def get_joint_pos(self, return_dict=True): + js = self.herej() + if return_dict: + ret = {k: v for k, v in zip(["j1", "j2", "j3", "j4", "j5", "j6"], js)} + else: + ret = js + return ret def get_flange_pos(self, frame=None, return_dict=True): return self.get_cartesian_pos(FLANGE, frame, return_dict=return_dict) @@ -1045,75 +1091,75 @@ class RobotTCP(TcpDevice, Stoppable): if value !=self.cartesian_motors_enabled: self.cartesian_motors_enabled = value if value: - for i in range(6): - self.cartesian_motors.append(RobotCartesianMotor(self, i)) + for i in ROBOT_CARTESIAN_MOTORS: + self.cartesian_motors[i] = RobotCartesianMotor(self, i) add_device(self.cartesian_motors[i], True) self.cartesian_destination = self.get_cartesian_pos(return_dict=True) self.cartesian_pos = self.cartesian_destination else: - for m in self.cartesian_motors: + for m in self.cartesian_motors.values(): remove_device(m) - self.cartesian_motors = [] + self.cartesian_motors = {} self.cartesian_destination = None else: if value: self.cartesian_destination = self.get_cartesian_pos(return_dict=True) - for m in self.cartesian_motors: + for m in self.cartesian_motors.values(): m.initialize() if "spherical" in pseudos: if value !=self.spherical_motors_enabled: self.spherical_motors_enabled = value if value: - for i in range(3): - self.spherical_motors.append(RobotSphericalMotor(self, i)) - add_device(self.spherical_motors[i], True) + for i in ROBOT_SPHERICAL_MOTORS: + self.spherical_motors[i] = RobotSphericalMotor(self, i) + add_device(self.spherical_motors[i], True) self.spherical_destination = self.get_spherical_pos() self.spherical_pos = self.spherical_destination else: - for m in self.spherical_motors: + for m in self.spherical_motors.values(): remove_device(m) - self.spherical_motors = [] + self.spherical_motors = {} self.spherical_destination = None else: if value: self.spherical_destination = self.get_spherical_pos() - for m in self.spherical_motors: + for m in self.spherical_motors.values(): m.initialize() if "linear" in pseudos: if value !=self.linear_axis_motors_enabled: self.linear_axis_motors_enabled = value if value: - self.linear_axis_motors.append(LinearAxisMotor(self, "z_lin")) - add_device(self.linear_axis_motors[0], True) + self.linear_axis_motors["z_lin"] = LinearAxisMotor(self, "z_lin") + add_device(self.linear_axis_motors["z_lin"], True) self.linear_axis_destination = self.get_linear_axis_pos() self.linear_axis_pos = self.linear_axis_destination else: - for m in self.linear_axis_motors: + for m in self.linear_axis_motors.values(): remove_device(m) self.linear_axis_motors = [] self.linear_axis_destination = None else: if value: self.linear_axis_destination = self.get_linear_axis_pos() - for m in self.linear_axis_motors: + for m in self.linear_axis_motors.values(): m.initialize() #Joint motors def set_joint_motors_enabled(self, value): if value !=self.joint_motors_enabled: self.joint_motors_enabled = value if value: - for i in range(6): - self.joint_motors.append(RobotJointMotor(self, i)) - add_device(self.joint_motors[i], True) + for i in ROBOT_JOINT_MOTORS: + self.joint_motors[i] = RobotJointMotor(self, i) + add_device(self.joint_motors[i], True) else: - for m in self.joint_motors: + for m in self.joint_motors.values(): remove_device(m) - self.joint_motors = [] + self.joint_motors = {} else: if value: - for m in self.joint_motors: + for m in self.joint_motors.values(): m.initialize() #Alignment def align(self, point = None, frame = None, desc = None):