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-11-29 09:37:22 +01:00
parent 5c857d030a
commit d7570bcaf3
39 changed files with 1881 additions and 65 deletions
Download Patch File Download Diff File Expand all files Collapse all files

73
script/devices/RobotMotors.py Normal file
View File

@@ -0,0 +1,73 @@
import ch.psi.pshell.device.PositionerConfig as PositionerConfig
ROBOT_MOTORS = ["x" , "y", "z", "rx", "ry", "rz"]
class RobotCartesianMotor (PositionerBase):
def __init__(self, robot, index):
PositionerBase.__init__(self, robot.name + "_" + ROBOT_MOTORS[index], PositionerConfig())
self.index = index
self.robot = robot
#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):
return float("nan") if self.robot.cartesian_destination is None else float(robot.cartesian_destination[self.index])
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
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])
ROBOT_JOINT_MOTORS = ["j1" , "j2", "j3", "j4", "j5", "j6"]
class RobotJointMotor (PositionerBase):
def __init__(self, robot, index):
PositionerBase.__init__(self, robot.name + "_" + ROBOT_JOINT_MOTORS[index], PositionerConfig())
self.index = index
self.robot = robot
def doInitialize(self):
self.setpoint = self.doReadReadback()
self.setCache(self.doRead(), None)
def doRead(self):
return self.setpoint
def doWrite(self, value):
print "Move " + ROBOT_JOINT_MOTORS[self.index] + " to " + str(value)
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
def doReadReadback(self):
return self.robot.herej()[self.index] if self.robot.joint_pos is None else float(self.robot.joint_pos[self.index])

68
script/devices/RobotSC.py
View File

@@ -1,9 +1,25 @@
TOOL_CALIBRATION = "tCalib"
TOOL_SUNA= "tSuna"
TOOL_DEFAULT= TOOL_SUNA
DESC_FAST = "mFast"
DESC_SLOW = "mSlow"
DESC_SCAN = "mScan"
DESC_DEFAULT = DESC_FAST
run("devices/RobotTCP")
simulation = True
simulation = False
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
def mount(self, puck, sample):
return self.execute('mount',segment, puck, sample)
@@ -22,10 +38,11 @@ class RobotSC(RobotTCP):
RobotTCP.doUpdate(self)
global simulation
if not simulation:
if self.state != State.Offline:
self.get_joint_forces()
for dev in [jf1, jf2, jf3, jf4,jf5, jf6, jfc]:
dev.update()
if joint_forces:
if self.state != State.Offline:
self.get_joint_forces()
for dev in [jf1, jf2, jf3, jf4,jf5, jf6, jfc]:
dev.update()
#print time.time() -start
def start_task(self, program, *args, **kwargs):
@@ -35,20 +52,30 @@ class RobotSC(RobotTCP):
def stop_task(self):
RobotTCP.stop_task(self)
#TODO: Restore safe position
def set_remote_mode(self):
robot.set_profile("remote")
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:
add_device(RobotSC("robot","129.129.126.92:1000"),force = True)
#add_device(RobotSC("robot","129.129.126.74:1000"),force = True)
#add_device(RobotSC("robot","129.129.126.92:1000"),force = True)
add_device(RobotSC("robot","129.129.110.99:1000"),force = True)
else:
add_device(RobotSC("robot", "129.129.126.100:1000"), force = True)
add_device(RobotSC("robot", "129.129.110.100:1000"), force = True)
robot.high_level_tasks = ["mount", "firstmount"]
robot.setPolling(20)
robot.setPolling(500)
#robot.set_monitor_speed(20)
@@ -79,10 +106,15 @@ class jfc(ReadonlyRegisterBase):
return float('NaN')
return (robot.joint_forces[1]+74)/4 + (robot.joint_forces[2]+30)/4 + (robot.joint_forces[4]-0.8)/0.2
add_device(jf1(), force = True)
add_device(jf2(), force = True)
add_device(jf3(), force = True)
add_device(jf4(), force = True)
add_device(jf5(), force = True)
add_device(jf6(), force = True)
add_device(jfc(), force = True)
if joint_forces:
add_device(jf1(), force = True)
add_device(jf2(), force = True)
add_device(jf3(), force = True)
add_device(jf4(), force = True)
add_device(jf5(), force = True)
add_device(jf6(), force = True)
add_device(jfc(), force = True)
"""

180
script/devices/RobotTCP.py
View File

@@ -1,5 +1,10 @@
import threading
FRAME_DEFAULT = "world"
run("devices/RobotMotors")
class RobotTCP(TcpDevice, Stoppable):
def __init__(self, name, server, timeout = 1000, retries = 1):
TcpDevice.__init__(self, name, server)
@@ -20,11 +25,36 @@ class RobotTCP(TcpDevice, Stoppable):
self.lock = threading.Lock()
self.joint_forces = None
self.current_task = None
self.high_level_tasks = []
self.high_level_tasks = []
self.cartesian_destination = None
self.cartesian_pos = None
self.joint_pos = None
self.cartesian_motors_enabled = False
self.cartesian_motors = []
self.joint_motors_enabled = False
self.joint_motors = []
self.tool = None
self.frame = FRAME_DEFAULT
self.updating = False
def doInitialize(self):
super(RobotTCP, self).doInitialize()
self.reset = True
def set_tool(self,tool):
self.tool = tool
if self.cartesian_motors_enabled:
self.cartesian_pos = self.get_cartesian_pos()
for m in self.cartesian_motors:
m.initialize()
m.update()
def get_tool(self):
return self.tool
def _sendReceive(self, msg_id, msg = "", timeout = None):
tx = self.header if (self.header != None) else ""
@@ -106,13 +136,13 @@ class RobotTCP(TcpDevice, Stoppable):
for i in range(size): ret.append(float(a[i]));
return ret
def get_trf(self, name="tcp_t"):
def get_trsf(self, name="tcp_t"):
a = self.execute('get_trf', name)
ret = []
for i in range(6): ret.append(float(a[i]))
return ret
def set_tr(self, l, name="tcp_t"):
def set_trsf(self, l, name="tcp_t"):
self.evaluate(name + "={" + str(l[0]) + ","+ str(l[1]) + ","+ str(l[2]) + ","+ str(l[3]) + ","+ str(l[4]) + ","+ str(l[5]) + "}")
def get_jnt(self, name="tcp_j"):
@@ -125,10 +155,26 @@ class RobotTCP(TcpDevice, Stoppable):
self.evaluate(name + "={" + str(l[0]) + ","+ str(l[1]) + ","+ str(l[2]) + ","+ str(l[3]) + ","+ str(l[4]) + ","+ str(l[5]) + "}")
def get_pnt(self, name="tcp_p"):
a = self.execute('get_pnt', name)
ret = []
for i in range(6): ret.append(float(a[i]))
return ret
#a = self.execute('get_pnt', name)
#ret = []
#for i in range(6): ret.append(float(a[i]))
#return ret
return self.get_trsf(name+".trsf")
#trsf = (x,y,z,rx,ry,rz)
#TODO: config = (shoulder, elbow, wrist)
def set_pnt(self, trsf, name="tcp_p", config=None):
self.set_trsf(trsf, name+".trsf")
def get_tool_trsf(self, name=None):
if name is None:
name = self.tool
return self.get_trsf(name+".trsf")
def set_tool_trsf(self, trsf, name=None):
if name is None:
name = self.tool
self.set_trsf(trsf, name+".trsf")
def eval_int(self, cmd):
self.evaluate("tcp_n=" + cmd)
@@ -152,7 +198,7 @@ class RobotTCP(TcpDevice, Stoppable):
def eval_trf(self, cmd):
self.evaluate("tcp_t=" + cmd)
return self.get_trf()
return self.get_trsf()
def eval_pnt(self, cmd):
self.evaluate("tcp_p=" + cmd)
@@ -165,16 +211,19 @@ class RobotTCP(TcpDevice, Stoppable):
return self.powered
def enable(self):
self.evaluate("enablePower()")
if not self.is_powered():
raise Exception("Cannot enable power")
self.evaluate("enablePower()")
time.sleep(1.0)
if not self.is_powered():
raise Exception("Cannot enable power")
#waits for power to be actually cut off
def disable(self):
self.evaluate("disablePower()", timeout=5000)
def get_monitor_speed(self):
self.speed = self.eval_int("getMonitorSpeed()")
return self.speed
def set_monitor_speed(self, speed):
ret = self.eval_int("setMonitorSpeed(" + str(speed) + ")")
@@ -185,6 +234,10 @@ class RobotTCP(TcpDevice, Stoppable):
def is_calibrated(self):
return self.eval_bool("isCalibrated()")
def save_program(self):
ret = self.execute('save', timeout=5000)
if str(ret) != "0": raise Exception("Error saving program: " + str(ret))
def _update_working_mode(self, mode, status):
if mode==1:
self.working_mode = "manual"
@@ -205,7 +258,7 @@ class RobotTCP(TcpDevice, Stoppable):
def read_working_mode(self):
try:
mode = self.eval_int("workingMode(tcp_a)")
status = int(self.get_var("tcp_a[0]"))
status = int(self.get_var("tcp_a[0]"))
self._update_working_mode(mode, status)
self._update_state()
except:
@@ -232,8 +285,8 @@ class RobotTCP(TcpDevice, Stoppable):
def resume(self):
self.evaluate("restartMove()")
def resetMotion(self):
self.evaluate("resetMotion()")
def reset_motion(self, joint=None):
self.evaluate("resetMotion()" if (joint is None) else ("resetMotion(" + joint + ")"))
def is_empty(self):
self.empty = self.eval_bool("isEmpty()")
@@ -243,7 +296,7 @@ class RobotTCP(TcpDevice, Stoppable):
def is_settled(self):
self.settled = self.eval_bool("isSettled()")
self._update_state()
return self.powered
return self.settled
def get_move_id(self):
return self.eval_int("getMoveId()")
@@ -289,7 +342,7 @@ class RobotTCP(TcpDevice, Stoppable):
return self.eval_float("distance(" + trsf1 + ", " + trsf2 + ")")
def distance_p(self, pnt1, pnt2):
return self.eval_float("distance(" + pnt1 + ", " + pnt2 + ")")
return self.eval_float("distance(" + pnt1 + ", " + pnt2 + ")")
def compose(self, pnt, frame, trsf):
return self.eval_pnt("compose(" + pnt + ", " + frame + ", " + trsf + ")")
@@ -297,16 +350,24 @@ class RobotTCP(TcpDevice, Stoppable):
def here(self, tool, frame):
return self.eval_pnt("here(" + tool + ", " + frame + ")")
def joint_to_point(self, tool, frame, joint):
def joint_to_point(self, tool, frame, joint="tcp_j"):
return self.eval_pnt("jointToPoint(" + tool + ", " + frame + ", " + joint +")")
def point_to_joint(self, tool, initial_joint, point):
def point_to_joint(self, tool, initial_joint="tcp_j", point="tcp_p"):
if self.eval_bool("pointToJoint(" + tool + ", " + initial_joint + ", " + point +", j)"):
return self.get_jnt()
def position(self, point, frame):
return self.eval_trf("position(" + point + ", " + frame + ")")
#Profile
def get_profile(self):
return self.execute('get_profile', timeout=2000)
def set_profile(self, name, pwd = ""):
self.execute('set_profile', str(name), str(pwd), timeout=5000)
#Task control
def task_create(self, program, *args, **kwargs):
program = str(program)
@@ -332,7 +393,8 @@ class RobotTCP(TcpDevice, Stoppable):
#waits for the task to be actually killed
def task_kill(self, name):
self.evaluate('taskKill("' + str(name)+ '")', timeout = 2000)
#self.evaluate('taskKill("' + str(name)+ '")', timeout = 5000)
self.execute('kill', str(name), timeout=5000)
def get_task_status(self, name):
code = self.eval_int('taskStatus("' + str(name)+ '")')
@@ -381,14 +443,83 @@ class RobotTCP(TcpDevice, Stoppable):
"speed": self.speed,
"empty": self.empty,
"settled": self.settled,
"task": self.current_task }, None)
#print time.time() - start
"task": self.current_task,
"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()
except:
if self.state != State.Offline:
print >> sys.stderr, "Update error: " + str(sys.exc_info()[1])
self.setState(State.Offline)
#Cartesian space
def get_cartesian_pos(self):
return self.eval_pnt("jointToPoint(" + self.tool + ", " + self.frame + ", herej())")
#self.set_jnt(self.herej())
#return self.joint_to_point(tool, frame)
def get_cartesian_destination(self):
return self.here(self.tool, self.frame)
def get_distance_to_pnt(self, name):
#self.here(self.tool, self.frame) #???
self.get_cartesian_pos()
return self.distance_p("tcp_p", name)
#Cartesian peudo-motors
def set_motors_enabled(self, value):
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))
add_device(self.cartesian_motors[i], True)
self.cartesian_destination = self.get_cartesian_destination()
else:
for m in self.cartesian_motors:
remove_device(m)
self.cartesian_motors = []
self.cartesian_destination = None
else:
if value:
self.cartesian_destination = self.get_cartesian_destination()
for m in self.cartesian_motors:
m.initialize()
#Cartesian peudo-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)
else:
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()
#High-level, exclusive motion task.
def start_task(self, program, *args, **kwargs):
tasks = [t for t in self.high_level_tasks]
@@ -410,8 +541,7 @@ class RobotTCP(TcpDevice, Stoppable):
self._update_state()
def stop_task(self):
#tasks = [t for t in self.high_level_tasks]
tasks = []
tasks = [t for t in self.high_level_tasks]
if (self.current_task is not None) and (not self.current_task in tasks):
tasks.append(self.current_task)
for task in tasks:
@@ -426,7 +556,7 @@ class RobotTCP(TcpDevice, Stoppable):
self.current_task = task
return task
return None
def on_event(self,ev):
pass