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

Changed robot.frame, tool, remote_allowed_recorded to be persistent FS adjustables

Browse Source
This commit is contained in:
gac-bernina
2024-03-15 13:27:51 +01:00
parent f94d997686
commit fc9f6b23fc
9 changed files with 324 additions and 246 deletions
Download Patch File Download Diff File Expand all files Collapse all files

4
config/variables.properties Normal file
View File

@@ -0,0 +1,4 @@
#Thu Mar 14 07:47:28 CET 2024
LastRunDate=240314
DaySequentialNumber=1
FileSequentialNumber=4

3
devices/robot_settings.properties Normal file
View File

@@ -0,0 +1,3 @@
#Wed Mar 13 18:55:09 CET 2024
tool=t_JF01T0
frame=f_4mRad

6
devices/test.properties Normal file
View File

@@ -0,0 +1,6 @@
#Wed Mar 13 21:14:11 CET 2024
num=null
huhu=null
hu=null
speed=null
frame=f_4mRad

241
script/devices/RobotBernina.py
View File

@@ -1,14 +1,5 @@
"""
--> eco has to take care of using the correctkeywords (i.e. r instead of t_det)
--> recording can be done with robot.record() similar to robot.simulate() taking the usual kwargs, issuing either a linear, joint or cartesian motion with record = True flag.
the flag then saves coordinates, tool, frame temporarily, starts motion command in thread, adds callback function to send an event, do an interpolation and save the interpolated values to save coordinates,
--> any motion command will automatically rset the motion
--> resent_motion should cancel the recording task / kill it
rename save_coordinates to checked / tested or so.
"""
import math
PVBASE = "SARES20-ROB:"
KNOWN_POINTS = P_PARK, P_HOME = "park", "home"
TASKS = MOVE_PARK, MOVE_HOME, TWEAK_X , TWEAK_Y = "movePark", "moveHome", "tweakX", "tweakY"
@@ -18,14 +9,6 @@ DESC_DEFAULT = DESCS[0]
FLANGE = "flange"
TOOLS = ["t_JF01T03", "t_JF07T32", ]
#TOOLS = ["t_Detector",]
TOOL_DEFAULT = TOOL_DET = TOOLS[0]
FRAMES = ["f_actualFrame", "f_direct", "f_2mRad", "f_4mRad", "f_6mRad", "f_8mRad"]
#FRAMES = ["f_actualFrame", "f_Experiment1", "f_Experiment2", "f_Experiment3"]
FRAME_DEFAULT = FRAMES[1]
DEFAULT_ROBOT_POLLING = 200
TASK_WAIT_ROBOT_POLLING = 50
DEFAULT_SPEED=100
@@ -39,158 +22,10 @@ run("devices/RobotTCP")
override_remote_safety = False
simulation = False
def dimensionality_decorator(func):
def decorated(self, *args):
if hasattr(self[0], "__len__"):
return Array([func(s_, *args) for s_ in self])
else:
return Array(func(self, *args))
return decorated
def indexing_decorator(func):
def decorated(self, v, *args):
nn = len(shape(v))
if (nn ==1):
if (type(v[0]) is bool):
return Array([s_ for s_, v_ in zip(self,v) if v_])
else:
return Array([self[v_] for v_ in v])
elif nn ==2:
if type(v[0]) is bool:
return Array([[s_ for s_, v_ in zip(self[n],v[n]) if v_ ] for n in range(nn)])
else:
ret = func(self, v, *args)
if hasattr(ret, "__len__"):
return Array(ret)
else:
return ret
return decorated
class Array(list):
def __init__(self, data):
super(Array, self).__init__(data)
self.getint = super(Array, self).__getitem__
@dimensionality_decorator
def __add__(self, v):
return add(self, v)
@dimensionality_decorator
def __sub__(self, v):
return sub(self, v)
@dimensionality_decorator
def __mul__(self, v):
return mul(self, v)
@dimensionality_decorator
def __div__(self, v):
return div(self, v)
@dimensionality_decorator
def interpolate(self, num):
return interpolate(self, num)
@dimensionality_decorator
def __lt__(self, v):
return lower_than(self, v)
@dimensionality_decorator
def __le__(self, v):
return lower_equal(self, v)
@dimensionality_decorator
def __gt__(self, v):
return greater_than(self, v)
@dimensionality_decorator
def __ge__(self, v):
return greater_equal(self, v)
@indexing_decorator
def __getitem__(self, v):
return super(Array, self).__getitem__(v)
@property
def shape(self):
return shape(self)
@property
def abs(self):
return arr_abs(self)
@property
def T(self):
return Array(transpose(self))
### math helper functions
def shape(self):
o = self
s = []
while hasattr(o, "__len__"):
s = s+[len(o)]
o = o[0]
return s
def transpose(a):
mm = len(a[0])
a_t = [[v[m] for v in a] for m in range(mm)]
return a_t
def arr_abs(a):
n = a.shape
if len(n)==1:
a_abs = [abs(e) for e in a]
elif len(n)==2:
a_abs = [[abs(e) for e in v] for v in a]
return Array(a_abs)
def interpolate(v,points_per_interval, round_to=2):
v_i=[[round(v[m]+1.*n/points_per_interval*(v[m+1]-v[m]), round_to) for n in range(points_per_interval)] for m in range(len(v)-1)]
ret = []
for i in v_i:
ret=ret+i
ret=ret+[v[-1]]
return ret
def add(list1, v):
if hasattr(v, "__len__"):
return [l+v_ for l, v_ in zip(list1, v)]
else:
return [l+v for l in list1]
def sub(list1, v):
if hasattr(v, "__len__"):
return [l-v_ for l, v_ in zip(list1, v)]
else:
return [l-v for l in list1]
def mul(list1, v):
if hasattr(v, "__len__"):
return [l*v_ for l, v_ in zip(list1, v)]
else:
return [l*v for l in list1]
def div(list1, v):
if hasattr(v, "__len__"):
return [1.*l/v_ for l, v_ in zip(list1, v)]
else:
return [1.*l/v for l in list1]
def lower_than(self, v):
if hasattr(v, "__len__"):
return [v_>s_ for v_, s_ in zip(v, self)]
else:
return [v>s_ for s_ in self]
def lower_equal(self, v):
if hasattr(v, "__len__"):
return [v_>=s_ for v_, s_ in zip(v, self)]
else:
return [v>=s_ for s_ in self]
def greater_than(self, v):
if hasattr(v, "__len__"):
return [s_>v_ for v_, s_ in zip(v, self)]
else:
return [s_>v for s_ in self]
def greater_equal(self, v):
if hasattr(v, "__len__"):
return [s_>=v_ for v_, s_ in zip(v, self)]
else:
return [s_>=v for s_ in self]
class RobotBernina(RobotTCP):
def __init__(self, name, server, timeout = 1000, retries = 1):
RobotTCP.__init__(self, name, server, timeout, retries)
self.invert_detector_rz = False
RobotTCP.__init__(self, name, server, timeout, retries)
self.invert_detector_rz = AdjustableFS(name = "invert_detector_rz", default_value=False)
self.set_tasks(TASKS)
self.set_known_points(KNOWN_POINTS)
self.setPolling(DEFAULT_ROBOT_POLLING)
@@ -198,18 +33,32 @@ class RobotBernina(RobotTCP):
self.last_command_position = None
self.override_remote_safety = override_remote_safety
temp = {"frame": Array([]), "tool": Array([]), "pos": Array([])}
self.remote_allowed_recorded = {"spherical": temp, "cartesian": temp, "joint": temp}
self.remote_allowed_deltas = {
default_remote_allowed_recorded = {"spherical": temp, "cartesian": temp, "joint": temp}
default_remote_allowed_deltas = {
"spherical": {'delta': 1., 'r': 50., 'gamma': 1.},
"cartesian": {'rx': 1., 'ry': 1., 'rz': 1., 'x': 1., 'y': 1., 'z': 1.},
"joint": {'j1': 1., 'j2': 1., 'j3': 1., 'j4': 1., 'j5': 1., 'j6': 1.},
}
self.remote_allowed_recorded = AdjustableFS(name="remote_allowed", default_value = default_remote_allowed_recorded)
self.remote_allowed_deltas = AdjustableFS(name="remote_allowed_deltas", default_value = default_remote_allowed_deltas)
def set_override_remote_safety(self, value):
self.override_remote_safety = bool(value)
self.override_remote_safety(bool(value))
def set_invert_detector_rz(self, value):
self.invert_detector_rz = bool(value)
self.invert_detector_rz = (bool(value))
def reset_recorded_motions(self, index=None, motion=None):
if any([index is None, motion is None]):
temp = {"frame": Array([]), "tool": Array([]), "pos": Array([])}
default_remote_allowed_recorded = {"spherical": temp, "cartesian": temp, "joint": temp}
self.remote_allowed_recorded(default_remote_allowed_recorded)
return "removed all recorded motions"
else:
rem = self.remote_allowed_recorded()
for k in ["pos", "tool", "frame"]:
rem[motion][k].pop[index]
self.remote_allowed_recorded(rem)
return "removed recorded " + motion + " motion with index " + str(index)
def remote_allowed(self, from_coordinates, to_coordinates, frame_trsf, tool_trsf, motion):
if self.override_remote_safety:
@@ -218,30 +67,31 @@ class RobotBernina(RobotTCP):
# spherical motors are missing rx ry rz, maybe even in simulation
a1 = [from_coordinates[k] for k in ks]
a2 = [to_coordinates[k] for k in ks]
ds = [self.remote_allowed_deltas[motion][k] for k in ks]
cds = [self.remote_allowed_deltas[motion][k] for k in ks]
rc = self.remote_allowed_recorded[motion]
ds = Array([self.remote_allowed_deltas()[motion][k] for k in ks])
cds = Array([self.remote_allowed_deltas()[motion][k] for k in ks])
rc = self.remote_allowed_recorded()[motion]
if len(rc["pos"]) > 0:
#preselection by frame with concatenated arrays
if "frame" in rc.keys():
fr = rc["frame"]
fr = Array(rc["frame"])
idxs = (fr-frame_trsf).abs < cds
idxs_b =[all(idx) for idx in idxs]
rc = {"pos": rc["pos"][idxs_b], "tool": rc["tool"][idxs_b]}
rc = {"pos": Array(rc["pos"])[idxs_b], "tool": Array(rc["tool"])[idxs_b]}
if len(rc["pos"]) > 0:
if "tool" in rc.keys():
tr = rc["tool"]
idxs = (tr-tool_trsf).abs < cds
idxs_b =[all(idx) for idx in idxs]
rc = {"pos": rc["pos"][idxs_b]}
rc = {"pos": Array(rc["pos"])[idxs_b]}
if len(rc["pos"]) > 0:
for tr in rc["pos"]:
idxs1 = (tr-a1).abs < ds
idxs2 = (tr-a2).abs < ds
idxs1 = (Array(tr)-a1).abs < ds
idxs2 = (Array(tr)-a2).abs < ds
idx1 = [all(idx) for idx in idxs1]
idx2 = [all(idx) for idx in idxs2]
if (any(idx1)) & (any(idx2)):
return True
print("finish")
return False
def general_motion(self, **kwargs):
@@ -265,7 +115,6 @@ class RobotBernina(RobotTCP):
self.reset_motion(msg = "RECORDING TRAJECTORY TO LIST OF ALLOWED REMOTE MOTIONS - USE HANDHELD TO FINISH MOTION")
if self.working_mode != "manual":
get_context().sendEvent("motion", "Current working mode is {}. Change it to manual using the keyswitch to record motions.".format(self.working_mode))
return True
if (sum([m in kwargs.keys() for m in ROBOT_JOINT_MOTORS])>0) & (sum([m in kwargs.keys() for m in ROBOT_SPHERICAL_MOTORS])==0) & (sum([m in kwargs.keys() for m in ROBOT_CARTESIAN_MOTORS])==0):
k = "joint"
move = self.move_joint
@@ -286,9 +135,11 @@ class RobotBernina(RobotTCP):
move(sync = True, **kwargs)
frame = Array(self.frame_trsf)
tool = Array(self.tool_trsf)
self.remote_allowed_recorded[k]["frame"].append(frame)
self.remote_allowed_recorded[k]["tool"].append(tool)
self.remote_allowed_recorded[k]["pos"].append(interp)
recorded = self.remote_allowed_recorded()
recorded[k]["frame"].append(frame)
recorded[k]["tool"].append(tool)
recorded[k]["pos"].append(interp)
self.remote_allowed_recorded(recorded)
self.on_recording_finished()
return True
@@ -355,7 +206,7 @@ class RobotBernina(RobotTCP):
z = r*math.cos(delta)*math.cos(gamma)
rz = -math.asin(-math.sin(-math.sin(gamma)*math.sin(-delta)*math.sqrt(1/(math.cos(-delta)**2+(math.sin(gamma)*math.sin(-delta))**2))))
rx, ry, rz = self.rad2deg([-delta,+gamma, rz])
if self.invert_detector_rz:
if self.invert_detector_rz():
rz = rz+180
if abs(rz/180) > 0:
rz = rz - (rz+180)/360*360
@@ -393,7 +244,7 @@ class RobotBernina(RobotTCP):
def point_reachable(self, point,tool=None, verbose=True):
if tool == None:
tool = self.tool
tool = self.tool()
self.herej()
reachable = self.eval_bool("pointToJoint(" + tool + ",tcp_j," + point + ",j)")
if verbose:
@@ -435,9 +286,9 @@ class RobotBernina(RobotTCP):
sim = []
if frame == None:
frame = self.frame
frame = self.frame()
if tool == None:
tool = self.tool
tool = self.tool()
if desc == None:
desc = self.default_desc
self.set_frame(frame, name="tcp_f_spherical", change_default=False)
@@ -487,9 +338,9 @@ class RobotBernina(RobotTCP):
"""
sim = []
if frame == None:
frame = self.frame
frame = self.frame()
if tool == None:
tool = self.tool
tool = self.tool()
if desc == None:
desc = self.default_desc
self.set_frame(frame, name="tcp_f_spherical", change_default=False)
@@ -578,7 +429,7 @@ class RobotBernina(RobotTCP):
if desc == None:
desc = self.default_desc
if tool == None:
tool = self.tool
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():
@@ -739,9 +590,9 @@ def make_archivable(Motor):
#robot.latency = 0.005
robot.set_default_desc(DESC_DEFAULT)
robot.default_speed = DEFAULT_SPEED
robot.set_frame(FRAME_DEFAULT)
robot.set_frame(FRAME_DEFAULT, name="tcp_f_spherical", change_default=False)
robot.set_tool(TOOL_DEFAULT)
robot.set_frame(robot.frame())
robot.set_frame(robot.frame(), name="tcp_f_spherical", change_default=False)
robot.set_tool(robot.tool())
robot.setPolling(DEFAULT_ROBOT_POLLING)
robot.get_current_point() #TODO: REMOVE WHEN CURRENT POINT REPORTED BY POLLING MESSAGE
robot.set_motors_enabled(True)

102
script/devices/RobotTCP.py
View File

@@ -3,10 +3,10 @@ import threading
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)
TcpDevice.__init__(self, name, server)
#DeviceBase.__init__(self, name, DeviceConfig({"frame": "f_4mRad"}))
self.on_poll_status = {}
self.on_poll_status_set = {}
self.timeout = timeout
@@ -47,11 +47,11 @@ class RobotTCP(TcpDevice, Stoppable):
self.linear_axis_motors = {}
self.joint_motors_enabled = False
self.joint_motors = {}
self.tool = None
self.tool = AdjustableFS(name="tool", default_value="t_JF01T03")
self.default_desc = None
self.tool_open = None
self.tool_trsf = [0.0] * 6
self.frame = FRAME_DEFAULT
self.frame = AdjustableFS(name="frame", default_value="f_4mRad")
self.frame_trsf = [0.0] * 6
self.polling_interval = 0.01
self.reset = True
@@ -70,7 +70,7 @@ class RobotTCP(TcpDevice, Stoppable):
self.reset = True
def set_tool(self, tool):
self.tool=tool
self.tool(tool)
self.set_str(tool, name="s_actualTool")
self.evaluate("t_actualTool=" + tool)
#self.tool_trsf = self.get_tool_trsf()
@@ -80,7 +80,7 @@ class RobotTCP(TcpDevice, Stoppable):
self.is_tool_open()
def get_tool(self):
return self.tool
return self.tool()
def set_frame(self, frame, name=None, change_default=True):
if name is not None:
@@ -88,24 +88,24 @@ class RobotTCP(TcpDevice, Stoppable):
if change_default:
self.set_str(frame, name="s_actualFrame")
self.evaluate("f_actualFrame" + "=" + frame)
self.frame=frame
self.frame(frame)
if self.cartesian_motors_enabled:
self.update()
self.set_motors_enabled(True)
self.waitCacheChange(5000)
def get_frame(self):
return self.frame
return self.frame()
def set_default_frame(self):
self.set_frame(FRAME_DEFAULT)
self.set_frame(self.frame())
def assert_tool(self, tool=None):
if tool is None:
if self.tool is None:
if self.tool() is None:
raise Exception("Tool is undefined")
elif self.tool != tool:
raise Exception("Invalid tool: " + self.tool)
elif self.tool() != tool():
raise Exception("Invalid tool: " + self.tool())
def set_default_desc(self,default_desc):
self.default_desc=default_desc
@@ -274,13 +274,13 @@ class RobotTCP(TcpDevice, Stoppable):
def get_tool_trsf(self, name=None):
if name is None:
self.assert_tool()
name = self.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
name = self.tool()
self.set_trsf(trsf, name+".trsf")
def eval_int(self, cmd):
@@ -488,7 +488,7 @@ class RobotTCP(TcpDevice, Stoppable):
if self.isSimulated():
self.simulated_point = ""
if desc is None: desc = self.default_desc
if tool is None: tool = self.tool
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")
@@ -508,7 +508,7 @@ class RobotTCP(TcpDevice, Stoppable):
if self.isSimulated():
self.simulated_point = ""
if desc is None: desc = self.default_desc
if tool is None: tool = self.tool
if tool is None: tool = self.tool()
if not is_string(point):
robot.set_pnt(point , "tcp_p")
point = "tcp_p"
@@ -526,7 +526,7 @@ class RobotTCP(TcpDevice, Stoppable):
if self.isSimulated():
self.simulated_point = ""
if desc is None: desc = self.default_desc
if tool is None: tool = self.tool
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 +")")
@@ -563,17 +563,17 @@ class RobotTCP(TcpDevice, Stoppable):
"""
#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
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
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
if tool is None: tool = self.tool()
self.tool_open = robot.eval_bool(tool + ".gripper")
return self.tool_open
@@ -589,26 +589,26 @@ class RobotTCP(TcpDevice, Stoppable):
return self.eval_float("distance(" + pnt1 + ", " + pnt2 + ")")
def compose(self, pnt, frame = None, trsf = "tcp_t"):
if frame is None: frame = self.frame
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
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
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 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
if frame is None: frame = self.frame()
return self.eval_trf("position(" + point + ", " + frame + ")")
#Profile
@@ -690,9 +690,9 @@ class RobotTCP(TcpDevice, Stoppable):
print "Communication resumed"
robot.set_default_desc(DESC_DEFAULT)
robot.default_speed = DEFAULT_SPEED
robot.set_frame(FRAME_DEFAULT)
robot.set_frame(FRAME_DEFAULT, name="tcp_f_spherical", change_default=False)
robot.set_tool(TOOL_DEFAULT)
robot.set_frame(self.frame())
robot.set_frame(self.frame(), name="tcp_f_spherical", change_default=False)
robot.set_tool(self.tool())
robot.setPolling(DEFAULT_ROBOT_POLLING)
if self.reset or (self.state==State.Offline):
self.check_task()
@@ -736,8 +736,8 @@ class RobotTCP(TcpDevice, Stoppable):
self.on_task_finished(cur_task, ret)
self.current_task, self.current_task_ret = None, ret
self.frame = sts[8]
self.tool = sts[9]
self.frame(sts[8])
self.tool(sts[9])
for i in range(6):
self.frame_trsf[i] = float(sts[10 + i])
for i in range(6):
@@ -765,16 +765,16 @@ class RobotTCP(TcpDevice, Stoppable):
self.on_poll_status.update({
"connected": self.connected,
"powered": self.powered,
"invert_detector_rz": self.invert_detector_rz,
"invert_detector_rz": self.invert_detector_rz(),
"override_remote_safety": self.override_remote_safety,
"speed": self.speed,
"settled": self.settled,
"task": self.current_task,
"mode": self.working_mode,
"status": self.status,
"frame": self.frame,
"frame": self.frame(),
"frame_coordinates": self.frame_trsf,
"tool": self.tool,
"tool": self.tool(),
"tool_coordinates": self.tool_trsf,
"cartesian_motors_enabled": self.cartesian_motors_enabled,
"spherical_motors_enabled": self.spherical_motors_enabled,
@@ -922,7 +922,7 @@ class RobotTCP(TcpDevice, Stoppable):
pos.update(self.joint_pos)
pos.update(self.linear_axis_pos)
self.on_poll_status = {
"invert_detector_rz": self.invert_detector_rz,
"invert_detector_rz": self.invert_detector_rz(),
"connected": self.connected,
"powered": self.powered,
"speed": self.speed,
@@ -931,8 +931,8 @@ class RobotTCP(TcpDevice, Stoppable):
"mode": self.working_mode,
"override_remote_safety": self.override_remote_safety,
"status": self.status,
"frame": self.frame,
"tool": self.tool,
"frame": self.frame(),
"tool": self.tool(),
"pos": pos,
"cartesian_motors_enabled": self.cartesian_motors_enabled,
"spherical_motors_enabled": self.spherical_motors_enabled,
@@ -977,25 +977,25 @@ class RobotTCP(TcpDevice, Stoppable):
def set_tool_coordinates(self, v, name=None):
if name is None:
name = self.tool
name = self.tool()
self.set_trsf(v, name=name + ".trsf")
if name == self.tool:
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
name = self.frame()
self.set_trsf(v, name=name + ".trsf")
if name == self.frame:
if name == self.frame():
self.set_trsf(v, name="f_actualFrame.trsf")
#Cartesian space
def get_cartesian_pos(self, tool=None, frame=None, return_dict=True):
if tool is None:
self.assert_tool()
tool = self.tool
tool = self.tool()
if frame is None:
frame = self.frame
frame = self.frame()
#Do not work
#self.evaluate("tcp_j=herej(); tcp_p=jointToPoint(" + tool + ", " + frame + ", tcp_j)")
self.evaluate("tcp_j=herej()")
@@ -1028,18 +1028,18 @@ class RobotTCP(TcpDevice, Stoppable):
def get_cartesian_destination(self, tool=None, frame=None,return_dict=True):
if tool is None:
self.assert_tool()
tool = self.tool
tool = self.tool()
if frame is None:
frame = self.frame
frame = self.frame()
#return self.here(tool, frame)
self.get_cartesian_pos(return_dict=return_dict)
def get_spherical_destination(self, tool=None, frame=None):
if tool is None:
self.assert_tool()
tool = self.tool
tool = self.tool()
if frame is None:
frame = self.frame
frame = self.frame()
#return self.here(tool, frame)
return self.get_spherical_pos()
@@ -1181,7 +1181,7 @@ class RobotTCP(TcpDevice, Stoppable):
m.initialize()
#Alignment
def align(self, point = None, frame = None, desc = None):
if frame is None: frame = self.frame
if frame is None: frame = self.frame()
self.assert_tool()
if desc is None: desc = self.default_desc
if point is None:
@@ -1190,9 +1190,9 @@ class RobotTCP(TcpDevice, Stoppable):
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)
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)
self.movej("tcp_p", self.tool(), desc, sync = True)
return np
#High-level, exclusive motion task.

34
script/elements/adjustables.py Normal file
View File

@@ -0,0 +1,34 @@
from json import load, dump
from os import path
import os
BASEPATH = "C:/dev/pshell/config/bernina_robot/adjustables_fs/"
class AdjustableFS:
def __init__(self, name=None, default_value=None, file_path=None):
if file_path is None:
file_path = BASEPATH + name
self.file_path = file_path
if not path.exists(self.file_path):
if not path.exists(path.dirname(self.file_path)):
os.mkdir(path.dirname(self.file_path))
self.write_value(default_value)
self.name = name
def get_current_value(self):
with open(self.file_path, "r") as f:
res = load(f)
return res["value"]
def write_value(self, value):
with open(self.file_path, "w") as f:
dump({"value": value}, f, indent=4)
def __call__(self, value=None):
if not value is None:
self.write_value(value)
else:
return self.get_current_value()
def __repr__(self):
return "Current value of {} at: ".format(self.name) + str(self.get_current_value())

147
script/elements/array.py Normal file
View File

@@ -0,0 +1,147 @@
def dimensionality_decorator(func):
def decorated(self, *args):
if hasattr(self[0], "__len__"):
return Array([func(s_, *args) for s_ in self])
else:
return Array(func(self, *args))
return decorated
def indexing_decorator(func):
def decorated(self, v, *args):
nn = len(shape(v))
if (nn ==1):
if (type(v[0]) is bool):
return Array([s_ for s_, v_ in zip(self,v) if v_])
else:
return Array([self[v_] for v_ in v])
elif nn ==2:
if type(v[0]) is bool:
return Array([[s_ for s_, v_ in zip(self[n],v[n]) if v_ ] for n in range(nn)])
else:
ret = func(self, v, *args)
if hasattr(ret, "__len__"):
return Array(ret)
else:
return ret
return decorated
class Array(list):
def __init__(self, data):
super(Array, self).__init__(data)
self.getint = super(Array, self).__getitem__
@dimensionality_decorator
def __add__(self, v):
return add(self, v)
@dimensionality_decorator
def __sub__(self, v):
return sub(self, v)
@dimensionality_decorator
def __mul__(self, v):
return mul(self, v)
@dimensionality_decorator
def __div__(self, v):
return div(self, v)
@dimensionality_decorator
def interpolate(self, num):
return interpolate(self, num)
@dimensionality_decorator
def __lt__(self, v):
return lower_than(self, v)
@dimensionality_decorator
def __le__(self, v):
return lower_equal(self, v)
@dimensionality_decorator
def __gt__(self, v):
return greater_than(self, v)
@dimensionality_decorator
def __ge__(self, v):
return greater_equal(self, v)
@indexing_decorator
def __getitem__(self, v):
return super(Array, self).__getitem__(v)
@property
def shape(self):
return shape(self)
@property
def abs(self):
return arr_abs(self)
@property
def T(self):
return Array(transpose(self))
### math helper functions
def shape(self):
o = self
s = []
while hasattr(o, "__len__"):
s = s+[len(o)]
o = o[0]
return s
def transpose(a):
mm = len(a[0])
a_t = [[v[m] for v in a] for m in range(mm)]
return a_t
def arr_abs(a):
n = a.shape
if len(n)==1:
a_abs = [abs(e) for e in a]
elif len(n)==2:
a_abs = [[abs(e) for e in v] for v in a]
return Array(a_abs)
def interpolate(v,points_per_interval, round_to=2):
v_i=[[round(v[m]+1.*n/points_per_interval*(v[m+1]-v[m]), round_to) for n in range(points_per_interval)] for m in range(len(v)-1)]
ret = []
for i in v_i:
ret=ret+i
ret=ret+[v[-1]]
return ret
def add(list1, v):
if hasattr(v, "__len__"):
return [l+v_ for l, v_ in zip(list1, v)]
else:
return [l+v for l in list1]
def sub(list1, v):
if hasattr(v, "__len__"):
return [l-v_ for l, v_ in zip(list1, v)]
else:
return [l-v for l in list1]
def mul(list1, v):
if hasattr(v, "__len__"):
return [l*v_ for l, v_ in zip(list1, v)]
else:
return [l*v for l in list1]
def div(list1, v):
if hasattr(v, "__len__"):
return [1.*l/v_ for l, v_ in zip(list1, v)]
else:
return [1.*l/v for l in list1]
def lower_than(self, v):
if hasattr(v, "__len__"):
return [v_>s_ for v_, s_ in zip(v, self)]
else:
return [v>s_ for s_ in self]
def lower_equal(self, v):
if hasattr(v, "__len__"):
return [v_>=s_ for v_, s_ in zip(v, self)]
else:
return [v>=s_ for s_ in self]
def greater_than(self, v):
if hasattr(v, "__len__"):
return [s_>v_ for v_, s_ in zip(v, self)]
else:
return [s_>v for s_ in self]
def greater_equal(self, v):
if hasattr(v, "__len__"):
return [s_>=v_ for v_, s_ in zip(v, self)]
else:
return [s_>=v for s_ in self]

2
script/local.py
View File

@@ -2,6 +2,8 @@
# Deployment specific global definitions - executed after startup.py
###################################################################################################
run("elements/array")
run("elements/adjustables")
run("devices/RobotBernina")
run("devices/Axis")
run ("motion/tools")

31
script/test/test.py Normal file
View File

@@ -0,0 +1,31 @@
from json import load, dump
from os import path
import os
class AdjustableFS:
def __init__(self, file_path, name=None, default_value=None):
self.file_path = file_path
if not path.exists(self.file_path):
if not path.exists(path.dirname(self.file_path)):
os.mkdir(path.dirname(self.file_path))
self._write_value(default_value)
self.name = name
def get_current_value(self):
with open(self.file_path, "r") as f:
res = load(f)
return res["value"]
def write_value(self, value):
with open(self.file_path, "w") as f:
dump({"value": value}, f, indent=4)
def __call__(self, value=None):
if not value is None:
self.write_value(value)
else:
return self.get_current_value()
def __repr__(self):
return "Current value of {} at: ".format(self.name) + str(self.get_current_value())