pshell_config/mxsc
0
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Browse Source
This commit is contained in:
gac-S_Changer
2017-12-06 15:16:07 +01:00
parent d7570bcaf3
commit dd1359858b
8 changed files with 161 additions and 101 deletions
Download Patch File Download Diff File Expand all files Collapse all files

21
script/devices/RobotMotors.py
View File

@@ -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])

16
script/devices/RobotSC.py
View File

@@ -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)

111
script/devices/RobotTCP.py
View File

@@ -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]