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

Creation

Browse Source
This commit is contained in:
gobbo_a
2023-01-31 08:32:53 +01:00
commit a0436b871a
27 changed files with 5835 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files
script/.DS_Store
Vendored
BIN
View File
Binary file not shown.
script/client/.history
View File
script/client/BerninaRobotClient.py
+90
View File
@@ -0,0 +1,90 @@
from PShellClient import PShellClient
import json
import time
import sys
class BerninaRobotClient(PShellClient):
def __init__(self, url):
PShellClient.__init__(self, url)
self.start_sse_event_loop_task(["state", "shell"])
self.state = self.get_state()
self.debug=False
def on_event(self, name, value):
if name == "state":
self.state = value
print ("State: ", value)
elif name == "shell":
if self.debug:
print ("> ", value)
def get_state(self):
self.state = PShellClient.get_state(self)
return self.state
def wait_ready(self):
count = 0
#Monitors event but polls every second just n case an event is missed
while (True):
if self.state != "Busy":
break
time.sleep(0.01)
count = count + 1
if count>=100:
count=0
self.get_state()
if self.state != "Ready":
raise Exception("Invalid state: " + str(self.state))
def start_cmd(self, cmd, *argv):
cmd = cmd + "("
for a in argv:
cmd = cmd + (("'" + a + "'") if type(a) is str else str(a) ) + ", "
cmd = cmd + ")"
ret = self.start_eval(cmd)
self.get_state()
return ret
def wait_cmd(self, cmd):
self.wait_ready()
result = self.get_result(cmd)
#print (result)
if result["exception"] is not None:
raise Exception(result["exception"] )
return result["return"]
def abort_cmd(self):
self.abort()
self.eval("robot.stop_task()&")
def get_robot_state(self):
return self.eval("robot.state&")
def get_robot_status(self):
status = self.eval("robot.take()&")
return status
def print_info(self):
print ("State: " + str(self.get_state()))
print ("Robot state: " + str(self.get_robot_state()))
print ("Robot status: " + str(self.get_robot_status()))
print ("")
#High-level commands
def move_park(self, wait=True):
cmd = self.start_cmd("robot.move_park",)
return self.wait_cmd(cmd) if wait else cmd
def move_home(self, wait=True):
cmd = self.start_cmd("robot.move_home")
return self.wait_cmd(cmd) if wait else cmd
def tweak_x(self, offset, wait=True):
cmd = self.start_cmd("robot.tweak_x", offset)
return self.wait_cmd(cmd) if wait else cmd
script/client/PShellClient.py
+601
View File
@@ -0,0 +1,601 @@
import threading
import time
import sys
import requests
import json
try:
from urllib import quote # Python 2
except ImportError:
from urllib.parse import quote # Python 3
try:
from sseclient import SSEClient
except:
SSEClient = None
class TimeoutException(Exception):
pass
class PShellClient:
def __init__(self, url):
if not url.endswith('/'):
url=url+"/"
self.url = url
self.sse_event_loop_thread = None
self.subscribed_events = None
self.event_callback = None
self.plot_defaults={"format":"png", "width":600, "height":400}
self.debug = False
def _get(self, url, stream=False):
url=self.url+url
if self.debug:
print ("GET " + url)
return requests.get(url=url, stream=stream)
def _put(self, url, json_data=None):
url=self.url+url
if self.debug:
print ("PUT " + url + " -> " + json.dumps(json_data))
return requests.put(url=url, json=json_data)
def _del(self, url):
url=self.url+url
if self.debug:
print ("DEL " + url)
return requests.delete(url=url)
def _get_response(self, response, is_json=True):
if self.debug==True or self.debug=="rx":
print (" -> " + str(response.status_code) + ((" - " + response.text) if self.debug=="rx" else ""))
try:
response.raise_for_status()
except:
print (response.text)
raise
return json.loads(response.text) if is_json else response.text
def _get_binary_response(self, response):
response.raise_for_status()
return response.raw.read()
def get_plot_defaults(self):
"""Return plot default properties.
Args:
Returns:
Dictionary
"""
return self.plot_defaults.copy()
def set_plot_defaults(self, defaults):
"""Update plot default properties.
Args:
Dictionary that will updated into the plot default properties.
Returns:
"""
self.plot_defaults.update(defaults)
def get_version(self):
"""Return application version.
Args:
Returns:
String with application version.
"""
return self._get_response(self._get("version"), False)
def get_config(self):
"""Return application configuration.
Args:
Returns:
Dictionary.
"""
return self._get_response(self._get("config"))
def get_state(self):
"""Return application state.
Args:
Returns:
String: Invalid, Initializing,Ready, Paused, Busy, Disabled, Closing, Fault, Offline
"""
return self._get_response(self._get("state"))
def wait_state(self, state, timeout = -1):
"""Wait application state equals.
Args:
state (string or list of strings)
Returns:
"""
if type(state)==str:
state = [state]
start = time.time()
while self.get_state() not in state:
if (timeout>=0) and ((time.time()-start)>timeout):
raise TimeoutException()
time.sleep(0.1)
def wait_state_not(self, state, timeout = -1):
"""Wait application state different than.
Args:
state (string or list of strings)
Returns:
"""
if type(state)==str:
state = [state]
start = time.time()
while self.get_state() in state:
if (timeout>=0) and ((time.time()-start)>timeout):
raise TimeoutException()
time.sleep(0.1)
def get_logs(self):
"""Return application logs.
Args:
Returns:
List of logs.
Format of each log: [date, time, origin, level, description]
"""
return self._get_response(self._get("logs"))
def get_history(self, index):
"""Access console command history.
Args:
index(int): Index of history entry (0 is the most recent)
Returns:
History entry
"""
return self._get_response(self._get("history/"+str(index)), False)
def get_script(self, path):
"""Return script.
Args:
path(str): Script path (absolute or relative to script folder)
Returns:
String with file contents.
"""
return self._get_response(self._get("script/"+str(path)), False)
def get_devices(self):
"""Return global devices.
Args:
Returns:
List of devices.
Format of each device record: [name, type, state, value, age]
"""
return self._get_response(self._get("devices"))
def abort(self, command_id=None):
"""Abort execution of command
Args:
command_id(optional, int): id of the command to be aborted.
if None (default), aborts the foreground execution.
Returns:
"""
if command_id is None:
self._get("abort")
else:
return self._get("abort/"+str(command_id))
def pause(self):
"""Pause execution of command
Args:
Returns:
"""
self._get("pause")
def resume(self):
"""Resume execution of command
Args:
Returns:
"""
self._get("resume")
def reinit(self):
"""Reinitialize the software.
Args:
Returns:
"""
self._get("reinit")
def stop(self):
"""Stop all devices implementing the 'Stoppable' interface.
Args:
Returns:
"""
self._get("stop")
def update(self):
"""Update all global devices.
Args:
Returns:
"""
self._get("update")
def eval(self,statement):
"""Evaluates a statement in the interpreter.
If the statement finishes by '&', it is executed in background.
Otherwise statement is executed in foreground (exclusive).
Args:
statement(str): input statement
Returns:
String containing the console return.
If an exception is produces in the interpretor, it is re-thrown here.
"""
statement = quote(statement)
return self._get_response(self._get("eval/"+statement), False)
def run(self,script, pars=None, background=False):
"""Executes script in the interpreter.
Args:
script(str): name of the script (absolute or relative to the script base folder). Extension may be omitted.
pars(optional, list or dict): if a list is given, it sets sys.argv for the script.
If a dict is given, it sets global variable for the script.
background(optional, bool): if True script is executed in background.
Returns:
Return value of the script.
If an exception is produces in the interpretor, it is re-thrown here.
"""
return self._get_response(self._put("run", {"script":script, "pars":pars, "background":background, "async":False }))
def start_eval(self,statement):
"""Starts evaluation of a statement in the interpreter.
If the statement finishes by '&', it is executed in background.
Otherwise statement is executed in foreground (exclusive).
Args:
statement(str): input statement
Returns:
Command id (int), which is used to retrieve command execution status/result (get_result).
"""
statement = quote(statement)
return int(self._get_response(self._get("evalAsync/"+statement), False))
def eval_json(self,statement):
"""Evaluates a statement in the interpreter.
Args:
statement(str): input statement
Returns:
Return object decoded from JSON string
"""
statement = quote(statement)
return self._get_response(self._get("eval-json/"+statement), True)
def set_var(self,name, value):
"""Sets interpreter variable.
Args:
name(str): variable name
value(obj): value - must be JSON compatible
Returns:
"""
data = {}
data["name"]=name
data["value"]=value
return self._get_response(self._put("set-var", data), False)
def start_run(self,script, pars=None, background=False):
"""Starts execution of a script in the interpreter.
Args:
script(str): name of the script (absolute or relative to the script base folder). Extension may be omitted.
pars(optional, list or dict): if a list is given, it sets sys.argv for the script.
If a dict is given, it sets global variable for the script.
background(optional, bool): if True script is executed in background.
Returns:
Command id (int), which is used to retrieve command execution status/result (get_result).
"""
return int(self._get_response(self._put("run", {"script":script, "pars":pars, "background":background, "async":True })))
def get_result(self, command_id=-1):
"""Gets status/result of a command executed asynchronously (start_eval and start_run).
Args:
command_id(optional, int): command id. If equals to -1 (default) return status/result of the foreground task.
Returns:
Dictionary with the fields: 'id' (int): command id
'status' (str): unlaunched, invalid, removed, running, aborted, failed or completed.
'exception' (str): if status equals 'failed', holds exception string.
'return' (obj): if status equals 'completed', holds return value of script (start_run)
or console return (start_eval)
"""
return self._get_response(self._get("result/"+str(command_id)))
def help(self, input = "<builtins>"):
"""Returns help or auto-completion strings.
Args:
input(optional, str): - ":" for control commands
- "<builtins>" for builtin functions
- "devices" for device names
- builtin function name for function help
- else contains entry for auto-completion
Returns:
List
"""
return self._get_response(self._get("autocompletion/" + input))
def get_contents(self, path=None):
"""Returns contents of data path.
Args:
path(optional, str): Path to data relative to data home path.
- Folder
- File
- File (data root) | internal path
- internal path (on currently open data root)
Returns:
List of contents
"""
return self._get_response(self._get("contents" + ("" if path is None else ( "/"+path))), False)
def get_data(self, path, type="txt"):
"""Returns data on a given path.
Args:
path(str): Path to data relative to data home path.
- File (data root) | internal path
- internal path (on currently open data root)
type(optional, str): txt, "json", "bin", "bs"
Returns:
Data accordind to selected format/.
"""
if type == "json":
return self._get_response(self._get("data-json/"+path), True)
elif type == "bin":
return self._get_binary_response(self._get("data-bin/"+path, stream=True))
elif type == "bs":
from collections import OrderedDict
bs = self._get_binary_response(self._get("data-bs/"+path, stream=True))
index=0
msg = []
for i in range(4):
size =int.from_bytes(bs[index:index+4], byteorder='big', signed=False)
index=index+4
msg.append(bs[index:index+size])
index=index+size
[main_header, data_header, data, timestamp] = msg
main_header = json.loads(main_header, object_pairs_hook=OrderedDict)
data_header = json.loads(data_header, object_pairs_hook=OrderedDict)
channel = data_header["channels"][0]
channel["encoding"] = "<" if channel.get("encoding", "little") else ">"
from bsread.data.helpers import get_channel_reader
channel_value_reader = get_channel_reader(channel)
return channel_value_reader(data)
return self._get_response(self._get("data" + ("" if path is None else ( "/"+path))), False)
def get_data_attrs(self, path):
return self._get_response(self._get("data-attr/"+path), True)
def get_data_info(self, path):
return self._get_response(self._get("data-info/"+path), True)
def get_scan_data(self, layout, path, group, device, type="txt"):
"""Returns scan data of a device.
Args:
layout(str): data layout
path(str): scan path
group(str): scan group
device(str): device name
type(optional, str): txt, "json", "bin"
Returns:
Data accordind to selected format.
"""
if layout is None or layout.strip()=="" or path is None:
raise Exception ("Invalid scan persistence path or layout")
path=path.replace("/", "<br>")
path=path.replace("|", "<p>")
group=group.replace("/", "<br>")
layout=layout.replace(".", "<br>")
url=layout+"/"+path+"/"+group+"/"+device
if type == "json":
url= "scandata-json/"+url
return self._get_response(self._get(url), True)
elif type == "bin":
url= "scandata-bin/"+url
return self._get_binary_response(self._get(url, stream=True))
url= "scandata/"+url
return self._get_response(self._get(url), False)
def get_plot_contexts(self):
"""Return list of plot contexts
Args:
Returns:
List of names
"""
return self._get_response(self._get("plots"))
def delete_plot_context(self, title):
"""
Delete a plotting context.
Args:
title(str): name of the plotting context
Returns:
"""
return self._get_response(self._del("plots/"+title), False)
def get_num_plots(self, title=None):
"""Return number of plots in a given plotting context.
Args:
title(str): name of the plotting context
Returns:
Number of plots
"""
if title is None:
title="null"
return int(self._get_response(self._get("plots/"+title)))
def get_plot(self, title=None, index=0, format="png", width=None, height=None):
"""Return a plot as a given image type.
Args:
title(str): name of the plotting context
index(int): plot index (0-based)
format(str): plot format ("jpg", "png", "gif", "tif")
width(int): plot width (if 0 gets plot staddard size)
height(int): plot height (if 0 gets plot staddard size)
Returns:
Image file byte array
"""
if title is None:
title="null"
if format is None:
format=self.plot_defaults["format"]
if width is None:
width=self.plot_defaults["width"]
if height is None:
height=self.plot_defaults["height"]
url = "plot/"+title+"/"+str(index)+"/"+format+"/"+str(width)+"/"+str(height)
return self._get_binary_response(self._get(url, stream=True))
def print_logs(self):
for l in self.get_logs():
print ("%s %s %-20s %-8s %s" % tuple(l))
def print_devices(self):
for l in self.get_devices():
print ("%-16s %-32s %-10s %-32s %s" % tuple(l))
def print_help(self, input = "<builtins>"):
for l in self.help(input):
print (l)
#Events
def _sse_event_loop_task(self):
try:
while True:
try:
messages = SSEClient(self.url+"events")
for msg in messages:
if (self.subscribed_events is None) or (msg.event in self.subscribed_events):
try:
value = json.loads(msg.data)
except:
value = str(msg.data)
self.event_callback(msg.event, value)
except IOError as e:
#print(e)
pass
except:
print("Error:", sys.exc_info()[1])
#raise
finally:
print ("Exit SSE loop task")
self.sse_event_loop_thread = None
def start_sse_event_loop_task(self, subscribed_events = None, event_callback = None):
"""
Initializes server event loop task.
Args:
subscribed_events: list of event names to subscribe to. If None subscribes to all.
event_callback: callback function. If None, self.on_event is called instead.
Usage example:
def on_event(name, value):
if name == "state":
print ("State changed: ", value)
elif name == "record":
print ("Received scan record: ", value)
pc.start_sse_event_loop_task(["state", "record"], on_event)
"""
self.event_callback = event_callback if event_callback is not None else self.on_event
self.subscribed_events = subscribed_events
if SSEClient is not None:
if self.sse_event_loop_thread is None:
self.sse_event_loop_thread = threading.Thread(target=self._sse_event_loop_task, \
args = (), \
kwargs={})
self.sse_event_loop_thread.daemon = True
self.sse_event_loop_thread.start()
else:
raise Exception ("sseclient library is not instlled: server events are not available")
def on_event(self, name, value):
pass
if __name__ == "__main__":
import socket
ps = PShellClient( "http://" + socket.gethostname() + ":8080")
print (ps.get_state())
script/client/sseclient.py
+163
View File
@@ -0,0 +1,163 @@
import codecs
import re
import time
import warnings
import six
import requests
# Technically, we should support streams that mix line endings. This regex,
# however, assumes that a system will provide consistent line endings.
end_of_field = re.compile(r'\r\n\r\n|\r\r|\n\n')
class SSEClient(object):
def __init__(self, url, last_id=None, retry=3000, session=None, chunk_size=1024, **kwargs):
self.url = url
self.last_id = last_id
self.retry = retry
self.chunk_size = chunk_size
# Optional support for passing in a requests.Session()
self.session = session
# Any extra kwargs will be fed into the requests.get call later.
self.requests_kwargs = kwargs
# The SSE spec requires making requests with Cache-Control: nocache
if 'headers' not in self.requests_kwargs:
self.requests_kwargs['headers'] = {}
self.requests_kwargs['headers']['Cache-Control'] = 'no-cache'
# The 'Accept' header is not required, but explicit > implicit
self.requests_kwargs['headers']['Accept'] = 'text/event-stream'
# Keep data here as it streams in
self.buf = u''
self._connect()
def _connect(self):
if self.last_id:
self.requests_kwargs['headers']['Last-Event-ID'] = self.last_id
# Use session if set. Otherwise fall back to requests module.
requester = self.session or requests
self.resp = requester.get(self.url, stream=True, **self.requests_kwargs)
self.resp_iterator = self.resp.iter_content(chunk_size=self.chunk_size)
# TODO: Ensure we're handling redirects. Might also stick the 'origin'
# attribute on Events like the Javascript spec requires.
self.resp.raise_for_status()
def _event_complete(self):
return re.search(end_of_field, self.buf) is not None
def __iter__(self):
return self
def __next__(self):
decoder = codecs.getincrementaldecoder(
self.resp.encoding)(errors='replace')
while not self._event_complete():
try:
next_chunk = next(self.resp_iterator)
if not next_chunk:
raise EOFError()
self.buf += decoder.decode(next_chunk)
except (StopIteration, requests.RequestException, EOFError) as e:
time.sleep(self.retry / 1000.0)
self._connect()
# The SSE spec only supports resuming from a whole message, so
# if we have half a message we should throw it out.
head, sep, tail = self.buf.rpartition('\n')
self.buf = head + sep
continue
# Split the complete event (up to the end_of_field) into event_string,
# and retain anything after the current complete event in self.buf
# for next time.
(event_string, self.buf) = re.split(end_of_field, self.buf, maxsplit=1)
msg = Event.parse(event_string)
# If the server requests a specific retry delay, we need to honor it.
if msg.retry:
self.retry = msg.retry
# last_id should only be set if included in the message. It's not
# forgotten if a message omits it.
if msg.id:
self.last_id = msg.id
return msg
if six.PY2:
next = __next__
class Event(object):
sse_line_pattern = re.compile('(?P<name>[^:]*):?( ?(?P<value>.*))?')
def __init__(self, data='', event='message', id=None, retry=None):
self.data = data
self.event = event
self.id = id
self.retry = retry
def dump(self):
lines = []
if self.id:
lines.append('id: %s' % self.id)
# Only include an event line if it's not the default already.
if self.event != 'message':
lines.append('event: %s' % self.event)
if self.retry:
lines.append('retry: %s' % self.retry)
lines.extend('data: %s' % d for d in self.data.split('\n'))
return '\n'.join(lines) + '\n\n'
@classmethod
def parse(cls, raw):
"""
Given a possibly-multiline string representing an SSE message, parse it
and return a Event object.
"""
msg = cls()
for line in raw.splitlines():
m = cls.sse_line_pattern.match(line)
if m is None:
# Malformed line. Discard but warn.
warnings.warn('Invalid SSE line: "%s"' % line, SyntaxWarning)
continue
name = m.group('name')
if name == '':
# line began with a ":", so is a comment. Ignore
continue
value = m.group('value')
if name == 'data':
# If we already have some data, then join to it with a newline.
# Else this is it.
if msg.data:
msg.data = '%s\n%s' % (msg.data, value)
else:
msg.data = value
elif name == 'event':
msg.event = value
elif name == 'id':
msg.id = value
elif name == 'retry':
msg.retry = int(value)
return msg
def __str__(self):
return self.data
script/client/test.py
+13
View File
@@ -0,0 +1,13 @@
from BerninaRobotClient import BerninaRobotClient
brc = BerninaRobotClient("http://saresb-cons-01.psi.ch:8080")
brc.print_info()
print brc.move_park()
print brc.move_home()
print brc.tweak_x(5)
print brc.move_park(wait=False)
brc.abort_cmd()
print brc.move_home()
script/devices/Axis.py
+27
View File
@@ -0,0 +1,27 @@
import requests
def axis_exec(camera, cmd):
if is_string(camera):
camera = get_device(camera)
url = camera.url
s="axis-cgi/"
ctrl_url = url[0:url.find(s) + len(s)] + "com/ptz.cgi?"
cmd_url=ctrl_url+cmd
response = requests.get(url=cmd_url)
if 200<=response.status_code<300:
return response.text.strip()
raise Exception(response.text)
def axis_move(camera, direction):
direction=str(direction)
if direction not in ("home", "up", "down", "left", "right", "upleft", "upright", "downleft", "downright", "stop"):
raise Exception("Invalid direction: " + direction)
return axis_exec(camera, "move="+direction)
def axis_rzoom(camera, steps):
steps= int(steps)
if steps<-9999 or steps>9999:
raise Exception("Invalid relative zoom steps: " + str(steps))
return axis_exec(camera, "rzoom="+str(steps))
script/devices/RobotBernina.py
+120
View File
@@ -0,0 +1,120 @@
KNOWN_POINTS = P_PARK, P_HOME = "park", "home"
TASKS = MOVE_PARK, MOVE_HOME, TWEAK_X = "movePark", "moveHome", "tweakX"
DESCS = DESC_FAST,DESC_SLOW, = "mFast", "mSlow"
DESC_DEFAULT = DESCS[0]
TOOLS = TOOL_DET = "tDet"
TOOL_DEFAULT = TOOLS[0]
DEFAULT_ROBOT_POLLING = 500
TASK_WAIT_ROBOT_POLLING = 50
DEFAULT_SPEED=20
run("devices/RobotTCP")
simulation = False
class RobotSC(RobotTCP):
def __init__(self, name, server, timeout = 1000, retries = 1):
RobotTCP.__init__(self, name, server, timeout, retries)
self.set_tasks(TASKS)
self.set_known_points(KNOWN_POINTS)
self.setPolling(DEFAULT_ROBOT_POLLING)
self.last_command_timestamp = None
self.last_command_position = None
self.setSimulated()
def move_home(self):
if not self.is_in_point(P_HOME):
self.start_task(MOVE_HOME)
self.wait_task_finished(TASK_WAIT_ROBOT_POLLING)
self.assert_home()
def move_park(self):
if not self.is_in_point(P_PARK):
self.start_task(MOVE_PARK)
self.wait_task_finished(TASK_WAIT_ROBOT_POLLING)
self.assert_park()
def tweak_x(self, offset):
self.start_task(TWEAK_X, offset)
self.wait_task_finished(TASK_WAIT_ROBOT_POLLING)
def on_event(self,ev):
#print "EVT: " + ev
pass
def on_change_working_mode(self, working_mode):
pass
def on_task_finished(self, task, ret):
if self.isSimulated():
if ret is not None and ret <0:
if task==MOVE_HOME: self.simulated_point=P_HOME
elif task==MOVE_PARK: self.simulated_point=P_PARK
else: self.simulated_point=None
def doUpdate(self):
#start = time.time()
RobotTCP.doUpdate(self)
def start_task(self, program, *args, **kwargs):
#TODO: Check safe position
self.task_name = program
self.task_args = args
self.task_kwargs = kwargs
ret = RobotTCP.start_task(self, program, *args, **kwargs)
def stop_task(self):
RobotTCP.stop_task(self)
#TODO: Restore safe position
def set_remote_mode(self):
self.set_profile("remote")
def set_local(self):
self.set_profile("default")
def is_park(self):
return self.is_in_point(P_PARK)
def is_home(self):
return self.is_in_point(P_HOME)
def is_cleared(self):
return self.get_current_point() is not None
def assert_park(self):
self.assert_in_point(P_PARK)
def assert_home(self):
self.assert_in_point(P_HOME)
def assert_cleared(self):
if not self.is_cleared():
raise Exception("Robot not in cleared position")
def wait_ready(self):
robot.waitState(State.Ready, 1000) #robot.state.assertReady()
if simulation:
add_device(RobotSC("robot","localhost:1000"),force = True)
else:
add_device(RobotSC("robot", "TellRobot6S:1000"), force = True)
#robot.latency = 0.005
robot.set_default_desc(DESC_DEFAULT)
robot.default_speed = DEFAULT_SPEED
robot.set_frame(FRAME_DEFAULT)
robot.set_tool(TOOL_DEFAULT)
robot.setPolling(DEFAULT_ROBOT_POLLING)
robot.get_current_point() #TODO: REMOVE WHEN CURRENT POINT REPORTED BY POLLING MESSAGE
#show_panel(robot)
script/devices/RobotMotors.py
+62
View File
@@ -0,0 +1,62 @@
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(self.doRead(), None)
def doRead(self):
return float("nan") if self.robot.cartesian_destination is None else float(self.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.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("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.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])
script/devices/RobotTCP.py
+959
View File
@@ -0,0 +1,959 @@
import threading
FRAME_DEFAULT = "world"
FLANGE = "flange"
MAX_NUMBER_PARAMETERS = 20
run("devices/RobotMotors")
class RobotTCP(TcpDevice, Stoppable):
def __init__(self, name, server, timeout = 1000, retries = 1):
TcpDevice.__init__(self, name, server)
self.timeout = timeout
self.retries = retries
self.header = None
self.trailer = "\n"
self.array_separator = '|'
self.cmd_separator = ' '
self.msg_id = 0
self.working_mode = "invalid"
self.status = "invalid"
self.powered = None
self.settled = None
self.empty = None
self.working_mode = None
self.status = None
self.lock = threading.Lock()
self.joint_forces = None
self.current_task = None
self.current_task_ret = None
self.high_level_tasks = []
self.known_points = []
self.current_points = []
self.cartesian_destination = 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.default_desc = None
self.tool_open = None
#self.tool_trsf = [0.0] * 6
self.frame = FRAME_DEFAULT
self.polling_interval = 0.01
self.reset = True
self.default_tolerance = 5
self.default_speed = 100
self.latency = 0
self.last_msg_timestamp = 0
self.task_start_retries = 3
self.exception_on_task_start_failure = True #Tasks may start and be finished when checked
self.simulated_point = ""
def doInitialize(self):
super(RobotTCP, self).doInitialize()
self.reset = True
def set_tool(self, tool):
self.tool = tool
#self.tool_trsf = self.get_tool_trsf()
self.evaluate("tcp_curtool=" + tool)
if self.cartesian_motors_enabled:
self.update()
self.set_motors_enabled(True)
self.is_tool_open()
def get_tool(self):
return self.tool
def set_frame(self, frame):
self.frame = frame
self.evaluate("tcp_curframe=" + frame)
if self.cartesian_motors_enabled:
self.update()
self.set_motors_enabled(True)
self.waitCacheChange(5000)
def get_frame(self):
return self.frame
def set_default_frame(self):
self.set_frame(FRAME_DEFAULT)
def assert_tool(self, tool=None):
if tool is None:
if self.tool is None:
raise Exception("Tool is undefined")
elif self.tool != tool:
raise Exception("Invalid tool: " + self.tool)
def set_default_desc(self,default_desc):
self.default_desc=default_desc
def get_default_desc(self):
return self.default_desc
def set_tasks(self,tasks):
self.high_level_tasks=tasks
def get_tasks(self):
return self.high_level_tasks
def set_known_points(self, points):
self.known_points=points
def get_known_points(self):
return self.known_points
def get_current_points(self, tolerance = None):
ret = self.is_in_points(*self.known_points, tolerance = tolerance)
current_points = []
for i in range(len(ret)):
if ret[i] == True:
current_points.append(self.known_points[i])
return current_points
def get_current_point(self, tolerance = None):
current_points = self.get_current_points(tolerance)
if (current_points is not None) and ( len(current_points) >0):
return current_points[0]
return None
def get_current_points_cached(self):
return self.current_points
def get_current_point_cached(self):
if (self.current_points is not None) and (len (self.current_points) >0):
return self.current_points[0]
return None
def assert_in_known_point(self, tolerance = None):
if self.get_current_point(tolerance) is None:
raise Exception ("Robot not in known point")
def _sendReceive(self, msg_id, msg = "", timeout = None):
if self.latency >0:
timespan = time.time() - self.last_msg_timestamp
if timespan<self.latency:
time.sleep(self.latency-timespan)
tx = self.header if (self.header != None) else ""
tx = tx + msg_id + " " + msg
if (len(tx)>150):
raise Exception("Exceeded maximum message size")
self.getLogger().finer("TX = '" + str(tx)+ "'")
if (self.trailer != None): tx = tx + self.trailer
if self.isSimulated():
return ""
rx = self.sendReceive(tx, msg_id, self.trailer , 0, self.timeout if timeout is None else timeout, self.retries)
self.last_msg_timestamp = time.time()
rx=rx[:-1] #Remove 0A
self.getLogger().finer("RX = '" + str(rx) + "'")
if rx[:3] != msg_id:
if (time.time()-start) >= timeout:
raise Exception("Received invalid message id: " + str(rx[:3]) + " - expecting:" + msg_id )
if len(rx)<4:
raise Exception("Invalid message size: " + str(len(rx)) )
if rx[3] == "*":
raise Exception(rx[4:])
return rx[4:]
def call(self, msg, timeout = None):
self.lock.acquire()
try:
id = "%03d" % self.msg_id
self.msg_id = (self.msg_id+1)%1000
return self._sendReceive(id, msg, timeout)
finally:
self.lock.release()
def execute(self, command, *args, **kwargs):
timeout = None if (kwargs is None) or (not kwargs.has_key("timeout")) else kwargs["timeout"]
msg = str(command)
if len(args)>MAX_NUMBER_PARAMETERS:
raise Exception("Exceeded maximum number of parameters")
for i in range(len(args)):
msg += (self.cmd_separator if (i==0) else self.array_separator) + str(args[i])
rx = self.call(msg, timeout)
if rx.count(self.array_separator)>0:
return rx.split(self.array_separator)
return rx
def evaluate(self, cmd, timeout=None):
ret = self.execute('eval', cmd, timeout=timeout)
if is_string(ret):
if ret.strip() != "": raise Exception(ret)
def get_var(self, name):
return self.execute('get_var', name)
#Makes app crash
#def get_str(self, name='s'):
# return self.execute('get_str', name)
def get_arr(self, name, size):
return self.execute('get_arr', name, size)
def get_bool(self, name = "tcp_b"):
return True if (self.execute('get_bool', name).strip() == '1') else False
def get_int(self, name ="tcp_n"):
return int(self.get_var(name))
def get_float(self, name ="tcp_n"):
return float(self.get_var(name))
def get_int_arr(self, size, name="tcp_a"):
# not working. A Jython bug in PyUnicaode?
#return [int(x) for x in self.get_arr("tcp_a", size)]
ret = []
a=self.get_arr(name, size)
for i in range(size):
ret.append(int(a[i]))
return ret
def get_float_arr(self, size, name="tcp_a"):
#return [float(x) for x in self.get_arr("tcp_a", size)]
a=self.get_arr(name, size)
ret = [];
for i in range(size): ret.append(float(a[i]));
return ret
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_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"):
a = self.execute('get_jnt', name)
ret = []
for i in range(6): ret.append(float(a[i]))
return ret
def set_jnt(self, l, name="tcp_j"):
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
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:
self.assert_tool()
name = self.tool
return self.get_trsf(name+".trsf")
def set_tool_trsf(self, trsf, name=None):
if name is None:
self.assert_tool()
name = self.tool
self.set_trsf(trsf, name+".trsf")
def eval_int(self, cmd):
if self.isSimulated():
return 0
self.evaluate("tcp_n=" + cmd)
return self.get_int()
def eval_float(self, cmd):
if self.isSimulated():
return 0.0
self.evaluate("tcp_n=" + cmd)
return self.get_float()
def eval_bool(self, cmd):
if self.isSimulated():
return False
self.evaluate("tcp_b=" + cmd)
return self.get_bool()
#def eval_str(self, cmd):
# self.evaluate("s=" + cmd)
# return self.get_str()
def eval_jnt(self, cmd):
self.evaluate("tcp_j=" + cmd)
return self.get_jnt()
def eval_trf(self, cmd):
self.evaluate("tcp_t=" + cmd)
return self.get_trsf()
def eval_pnt(self, cmd):
self.evaluate("tcp_p=" + cmd)
return self.get_pnt()
#Robot control
def is_powered(self):
self.powered = self.eval_bool("isPowered()")
return self.powered
def enable(self):
if not self.is_powered():
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) + ")")
if (ret==-1): raise Exception("The robot is not in remote working mode")
if (ret==-2): raise Exception("The monitor speed is under the control of the operator")
if (ret==-3): raise Exception("The specified speed is not supported")
def set_default_speed(self):
set_monitor_speed(self.default_speed)
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):
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"
else:
self.working_mode = "invalid"
self.status = "invalid"
if self.working_mode != cur_mode:
try:
self.on_change_working_mode(self.working_mode)
except:
pass
def read_working_mode(self):
try:
mode = self.eval_int("workingMode(tcp_a)")
status = int(self.get_var("tcp_a[0]"))
self._update_working_mode(mode, status)
self._update_state()
except:
self.working_mode = "invalid"
self.status = "invalid"
return self.working_mode
def get_emergency_stop_sts(self):
st = self.eval_int("esStatus()")
if (st== 1): return "active"
if (st== 2): return "activated"
return "off"
def get_safety_fault_signal(self):
fault = self.eval_bool("safetyFault(s)")
#if (fault):
# return get_str()
return fault
#Motion control
def stop(self):
self.evaluate("stopMove()")
def resume(self):
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)
def is_empty(self):
self.empty = self.eval_bool("isEmpty()")
self._update_state()
return self.empty
def is_settled(self):
self.settled = self.eval_bool("isSettled()")
self._update_state()
return self.settled
def get_move_id(self):
return self.eval_int("getMoveId()")
def set_move_id(self, id):
return self.evaluate("setMoveId(" + str(id) + " )")
def get_joint_forces(self):
try:
self.evaluate("getJointForce(tcp_a)")
self.joint_forces = self.get_float_arr(6)
return self.joint_forces
except:
self.joint_forces = None
raise
def movej(self, joint_or_point, tool=None, desc=None, sync=False):
if self.isSimulated():
self.simulated_point = ""
if desc is None: desc = self.default_desc
if tool is None: tool = self.tool
#If joint_or_point is a list assumes ir is a point
if not is_string(joint_or_point):
robot.set_pnt(joint_or_point , "tcp_p")
joint_or_point = "tcp_p"
#TODO: in new robot movel and movej is freezing controller
#ret = self.eval_int("movej(" + joint_or_point + ", " + tool + ", " + desc +")")
ret = int(self.execute('movej',joint_or_point, tool, desc))
if sync:
self.wait_end_of_move()
if self.isSimulated():
self.simulated_point = joint_or_point
return ret
def movel(self, point, tool=None, desc=None, sync=False):
if self.isSimulated():
self.simulated_point = ""
if desc is None: desc = self.default_desc
if tool is None: tool = self.tool
if not is_string(point):
robot.set_pnt(point , "tcp_p")
point = "tcp_p"
#TODO: in new robot movel and movej is freezing controller
#ret = self.eval_int("movel(" + point + ", " + tool + ", " + desc +")")
ret = int(self.execute('movel',point, tool, desc))
if sync:
self.wait_end_of_move()
if self.isSimulated():
self.simulated_point = point
return ret
def movec(self, point_interm, point_target, tool=None, desc=None, sync=False):
if self.isSimulated():
self.simulated_point = ""
if desc is None: desc = self.default_desc
if tool is None: tool = self.tool
#TODO: in new robot movel and movej is freezing controller
#ret = self.eval_int("movec(" + point_interm + ", " + point_target + ", " + tool + ", " + desc +")")
ret = int(self.execute('movec', point_interm, point_target, tool, desc))
if sync:
self.wait_end_of_move()
if self.isSimulated():
self.simulated_point = point_target
return ret
def wait_end_of_move(self):
time.sleep(0.05)
self.update()
#time.sleep(0.01)
#self.waitCacheChange(-1)
self.waitReady(-1)
#self.waitState(State.Ready, -1)
#Tool - synchronized as can freeze communication
"""
def open_tool(self, tool=None, timeout=3000):
#This function can timeout since it synchronizes move. Checking state before otherwise it can freeze the communication.
self.waitState(State.Ready, -1)
if tool is None: tool = self.tool
return self.evaluate("open(" + tool + " )", timeout=timeout)
def close_tool(self, tool=None, timeout=3000):
#This function can timeout since it synchronizes move. Checking state before otherwise it can freeze the communication.
self.waitState(State.Ready, -1)
if tool is None: tool = self.tool
return self.evaluate("close(" + tool + " )", timeout=timeout)
"""
#Tool - Not synchronized calls: atention to open/close only when state is Ready
def open_tool(self, tool=None):
if tool is None: tool = self.tool
self.evaluate(tool + ".gripper=true")
self.tool_open = True
def close_tool(self, tool=None):
if tool is None: tool = self.tool
self.evaluate(tool + ".gripper=false")
self.tool_open = False
def is_tool_open(self, tool=None):
if tool is None: tool = self.tool
self.tool_open = robot.eval_bool(tool + ".gripper")
return self.tool_open
#Arm position
def herej(self):
return self.eval_jnt("herej()")
def distance_t(self, trsf1, trsf2):
return self.eval_float("distance(" + trsf1 + ", " + trsf2 + ")")
def distance_p(self, pnt1, pnt2):
return self.eval_float("distance(" + pnt1 + ", " + pnt2 + ")")
def compose(self, pnt, frame = None, trsf = "tcp_t"):
if frame is None: frame = self.frame
return self.eval_pnt("compose(" + pnt + ", " + frame + ", " + trsf + ")")
def here(self, tool=None, frame=None):
if tool is None: tool = self.tool
if frame is None: frame = self.frame
return self.eval_pnt("here(" + tool + ", " + frame + ")")
def joint_to_point(self, tool=None, frame=None, joint="tcp_j"):
if tool is None: tool = self.tool
if frame is None: frame = self.frame
return self.eval_pnt("jointToPoint(" + tool + ", " + frame + ", " + joint +")")
def point_to_joint(self, tool=None, initial_joint="tcp_j", point="tcp_p"):
if tool is None: tool = self.tool
if self.eval_bool("pointToJoint(" + tool + ", " + initial_joint + ", " + point +", j)"):
return self.get_jnt()
def position(self, point, frame=None):
if frame is None: frame = self.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)
priority = 10 if (kwargs is None) or (not kwargs.has_key("priority")) or (kwargs["priority"] is None) else kwargs["priority"]
name = str(program if (kwargs is None) or (not kwargs.has_key("name")) else kwargs["name"])
if self.get_task_status(name)[0] != -1:
raise Exception("Task already exists: " + name)
if priority<1 or priority > 100:
raise Exception("Invalid priority: " + str(priority))
cmd = program + '('
for i in range(len(args)):
val = args[i]
if type(val) == bool:
if val == True: val = "true"
elif val == False: val = "false"
cmd += str(val) + (',' if (i<(len(args)-1)) else '')
cmd+=')'
#TODO: in new robot exec taskCreate is freezing controller
#REMOVE if bug is fixed
self.execute('task_create',name, str(priority), program, *args)
#self.evaluate('taskCreate "' + name + '", ' + str(priority) + ', ' + cmd)
if self.isSimulated():
self.simulated_point = ""
def task_suspend(self, name):
self.evaluate('taskSuspend("' + str(name)+ '")')
#waits until the task is ready for restart
def task_resume(self, name):
self.evaluate('taskResume("' + str(name)+ '",0)', timeout = 2000)
#waits for the task to be actually killed
def task_kill(self, name):
#self.evaluate('taskKill("' + str(name)+ '")', timeout = 5000)
self.execute('kill', str(name), timeout=5000)
def get_task_status(self, name):
if self.isSimulated():
return (-1,"Stopped")
code = self.eval_int('taskStatus("' + str(name)+ '")')
#TODO: String assignments in $exec causes application to freeze
#status = self
if code== -1: status = "Stopped"
elif code== 0: status = "Paused"
elif code== 1: status = "Running"
#else: status = self.execute('get_help', code)
else: status = "Error"
return (code,status)
def get_tasks_status(self, *pars):
ret = self.execute("task_sts", *pars)
ret = ret[0:len(pars)]
for i in range(len(ret)):
try:
ret[i] = int(ret[i])
except:
ret[i] = None
return ret
def _update_state(self):
#self.setState(State.Busy if self.status=="move" else State.Ready)
if self.state==State.Offline:
print "Communication resumed"
if self.reset or (self.state==State.Offline):
self.check_task()
if self.current_task is not None:
print "Ongoing task: " + self.current_task
if (not self.settled) or (self.current_task is not None): self.setState(State.Busy)
elif not self.empty: self.setState(State.Paused)
else: self.setState(State.Ready)
def doUpdate(self):
try:
start = time.time()
cur_task = self.current_task #Can change in background so cache it
if self.isSimulated():
sts = [1, 1,"1", 100, "1", "1", 0, 0, \
0, 0, 0, 0, 0, 0, \
0, 0, 0, 0, 0, 0, \
]
else:
sts = self.execute("get_status", cur_task)
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"
#TODO: add tool open
if cur_task is not None:
if self.isSimulated():
if time.time() - self.current_task_timestamp > 3.0:
log("Task "+ str(cur_task) + " finished with code: -1", False)
ret = -1 if self.current_task_timestamp>0 else None
self.on_task_finished(self.current_task, ret)
self.current_task, self.current_task_ret = None, ret
else:
if int(sts[6]) < 0:
try:
ret = int(sts[7])
except:
ret = None
log("Task "+ str(cur_task) + " finished with code: " + str(ret), False)
self.on_task_finished(cur_task, ret)
self.current_task, self.current_task_ret = None, ret
for i in range(6):
self.joint_pos[i] = float(sts[8 + i])
for i in range(6):
self.cartesian_pos[i] = float(sts[14 + i])
ev_index = 20 #7
ev = sts[ev_index] if len(sts)>ev_index else ""
if len(ev.strip()) >0:
self.getLogger().info(ev)
self.on_event(ev)
self._update_state()
self.reset = False
self.setCache({"powered": self.powered,
"speed": self.speed,
"empty": self.empty,
"settled": self.settled,
"task": cur_task,
"mode": self.working_mode,
"status": self.status,
"open": self.tool_open,
"pos": self.get_current_point_cached() if self.state==State.Ready else None #TODO: make it calculated in robot by polling funtion
}, None)
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)
#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):
if tool is None:
self.assert_tool()
tool = self.tool
if frame is None:
frame = self.frame
#Do not work
#self.evaluate("tcp_j=herej(); tcp_p=jointToPoint(" + tool + ", " + frame + ", tcp_j)")
#return self.get_pnt()
a = self.execute('get_pos', tool, frame)
ret = []
for i in range(6): ret.append(float(a[i]))
return ret
def get_flange_pos(self, frame=None):
return get_cartesian_pos(FLANGE, frame)
def get_cartesian_destination(self, tool=None, frame=None):
if tool is None:
self.assert_tool()
tool = self.tool
if frame is None:
frame = self.frame
return self.here(tool, frame)
def get_distance_to_pnt(self, name):
if self.isSimulated():
if name and (name==self.simulated_point):
return 0
else:
return 10000
#self.here(self.tool, self.frame) #???
self.set_pnt(self.get_cartesian_pos() )
return self.distance_p("tcp_p", name)
def is_in_point(self, p, tolerance = None): #Tolerance in mm
if (tolerance is None) and p in self.known_points:
#If checking a known point with default tolerance, updates the position cache
return p in self.get_current_points()
tolerance = self.default_tolerance if tolerance == None else tolerance
d = self.get_distance_to_pnt(p)
if d<0:
raise Exception ("Error calculating distance to " + p + " : " + str(d))
return d<tolerance
def get_distance_to_pnts(self, *pars):
if self.isSimulated():
return [self.get_distance_to_pnt(p) for p in pars]
ret = self.execute("dist_pnt", *pars)
ret = ret[0:len(pars)]
for i in range(len(ret)):
try:
ret[i] = float(ret[i])
except:
ret[i] = None
return ret
def is_in_points(self, *pars, **kwargs): #Tolerance in mm
tolerance = self.default_tolerance if (kwargs is None) or (not kwargs.has_key("tolerance")) or (kwargs["tolerance"] is None) else kwargs["tolerance"]
ret = self.get_distance_to_pnts(*pars)
for i in range(len(ret)):
if ret[i]<0:
ret[i] = None
else:
ret[i] = ret[i]<tolerance
if (tolerance == self.default_tolerance) and (set(self.known_points).issubset(set(pars))): #Only update cache if tolerance is default
current_points = []
for i in range(len(ret)):
if ret[i] == True:
current_points.append(self.known_points[i])
self.current_points = current_points
return ret
def assert_in_point(self, p, tolerance = None): #Tolerance in mm
if not self.is_in_point(p, tolerance):
raise Exception ("Not in position " + p)
#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 = []
else:
if value:
for m in self.joint_motors:
m.initialize()
#Alignment
def align(self, point = None, frame = None, desc = None):
if frame is None: frame = self.frame
self.assert_tool()
if desc is None: desc = self.default_desc
if point is None:
self.set_pnt(self.get_cartesian_pos() )
else:
self.set_pnt(point)
np=self.eval_trf('align(tcp_p.trsf, ' + frame + '.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]
if (self.current_task is not None) and (not self.current_task in tasks):
tasks.append(self.current_task)
if not program in tasks:
tasks.append(program)
#for task in tasks:
# if self.get_task_status(task)[0]>=0:
# raise Exception("Ongoing high-level task: " + task)
ts = self.get_tasks_status(*tasks)
for i in range(len(ts)):
if ts[i] > 0:
raise Exception("Ongoing high-level task: " + tasks[i])
self.clear_task_ret()
for i in range(self.task_start_retries):
self.task_create(program, *args, **kwargs)
(code, status) = self.get_task_status(program)
if code>=0: break
if self.isSimulated():
break
if i < self.task_start_retries-1:
ret = self.get_task_ret(False)
if ret == 0: #Did't start
log("Retrying starting : " + str(program) + str(args) + " - status: " + str(status) + " (" + str(code) + ")", False)
print "Retrying starting : " + str(program) + str(args) + " - status: " + str(status) + " (" + str(code) + ")"
else :
print "Retrying aborted : " + str(program) + str(args) + " - ret: " + str(ret) + " - status: " + str(status) + " (" + str(code) + ")"
break
else:
log("Failed starting : " + str(program) + str(args), False)
print "Failed starting : " + str(program) + str(args)
if self.exception_on_task_start_failure:
raise Exception("Cannot start task: " + program + str(args))
log("Task started: " + str(program) + str(args) + " - status: " + str(status) + " (" + str(code) + ")", False)
self.current_task, self.current_task_ret, self.current_task_timestamp = program, None, time.time()
self.update()
#self._update_state()
#TODO: remove
if self.current_task is None:
log("Task finished in first polling : " + str(program) , False)
print "Task finished in first polling : " + str(program)
return code
def stop_task(self):
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:
#if self.get_task_status(task)[0]>=0:
self.task_kill(task)
if self.isSimulated():
self.current_task_timestamp=-1
self.reset_motion()
def get_task_ret(self, cached = True):
return self.current_task_ret if cached else self.eval_int("tcp_ret")
def clear_task_ret(self):
return self.evaluate("tcp_ret=0")
def get_task(self):
return self.current_task
def check_task(self):
if self.current_task is None:
for task in self.high_level_tasks:
if self.get_task_status(task)[0]>=0:
self.current_task, self.current_task_ret = task, None
log("Task detected: " + str(self.current_task), False)
return self.current_task
def wait_task_finished (self, polling = None):
cur_polling = self.polling
if polling is not None:
self.polling = polling
try:
while self.get_task() != None:
time.sleep(self.polling_interval)
finally:
if polling is not None:
self.polling = cur_polling
ret = self.get_task_ret()
return ret
def assert_no_task(self):
task = self.check_task()
if task != None:
raise Exception("Ongoing task: " + task)
def on_event(self,ev):
pass
def on_change_working_mode(self, working_mode):
pass
def on_task_finished(self, task, ret):
pass
script/local.groovy
+3
View File
@@ -0,0 +1,3 @@
///////////////////////////////////////////////////////////////////////////////////////////////////
// Deployment specific global definitions - executed after startup.groovy
///////////////////////////////////////////////////////////////////////////////////////////////////
script/local.js
+4
View File
@@ -0,0 +1,4 @@
///////////////////////////////////////////////////////////////////////////////////////////////////
// Deployment specific global definitions - executed after startup.js
///////////////////////////////////////////////////////////////////////////////////////////////////
script/local.py
+6
View File
@@ -0,0 +1,6 @@
###################################################################################################
# Deployment specific global definitions - executed after startup.py
###################################################################################################
run("devices/RobotBernina")
run("devices/Axis")