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

Fixed some issues with recording trajectories for remote motion

Browse Source
This commit is contained in:
gac-bernina
2024-02-27 14:52:09 +01:00
parent 253db3b21d
commit 1f879d7c3f
2 changed files with 76 additions and 43 deletions
Download Patch File Download Diff File Expand all files Collapse all files
script/devices/RobotBernina.py
+58 -38
View File
@@ -36,8 +36,8 @@ ROBOT_SPHERICAL_MOTORS = ["r" , "gamma", "delta"]
run("devices/RobotTCP")
simulation = False
override_remote_safety = False
simulation = True
def dimensionality_decorator(func):
def decorated(self, *args):
@@ -104,6 +104,9 @@ class Array(list):
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
@@ -120,6 +123,14 @@ def transpose(a):
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 = []
@@ -184,41 +195,49 @@ class RobotBernina(RobotTCP):
self.setPolling(DEFAULT_ROBOT_POLLING)
self.last_command_timestamp = None
self.last_command_position = None
self.override_remote_safety = override_remote_safety
temp = {"frame": Array([]), "tool": Array([]), "pos": Array([])}
self.remote_allowed = {"spherical": temp, "cartesian": temp, "joint": temp}
self.remote_allowed_recorded = {"spherical": temp, "cartesian": temp, "joint": temp}
self.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.},
}
def set_override_remote_safety(self, value):
self.override_remote_safety = bool(value)
def remote_allowed(self, from_coordinates, to_coordinates, frame_trsf, tool_trsf, motion):
if self.override_remote_safety:
return True
ks = ROBOT_CARTESIAN_MOTORS if motion == "cartesian" else ROBOT_SPHERICAL_MOTORS if motion == "spherical" else ROBOT_JOINT_MOTORS
# 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["cartesian"][k] for k in ROBOT_CARTESIAN_MOTORS]
rc = self.remote_allowed[motion]
#preselection by frame with concatenated arrays
if "frame" in rc.keys():
fr = rc["frame"]
idxs = (fr-frame_trsf) < cds
idxs_b =[all(idx) for idx in idxs]
rc = {"pos": rc["pos"][idxs_b], "tool": rc["tool"][idxs_b]}
if "tool" in rc.keys():
tr = rc["tool"]
idxs = (tr-tool_trsf) < cds
idxs_b =[all(idx) for idx in idxs]
rc = {"pos": rc["pos"][idxs_b]}
for tr in rc["pos"]:
idxs1 = (tr-a1) < ds
idxs2 = (tr-a2) < ds
idx1 = [all(idx) for idx in idxs1]
idx2 = [all(idx) for idx in idxs2]
if (any(idx1)) & (any(idx2)):
return True
cds = [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"]
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]}
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]}
if len(rc["pos"]) > 0:
for tr in rc["pos"]:
idxs1 = (tr-a1).abs < ds
idxs2 = (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
return False
def general_motion(self, **kwargs):
@@ -242,7 +261,7 @@ 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
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
@@ -255,14 +274,17 @@ class RobotBernina(RobotTCP):
else:
get_context().sendEvent("motion", "\nNo unique coordinate system found for these motions use only unique keywords out of:\n {}\n {}\n {}.".format(ROBOT_JOINT_MOTORS, ROBOT_CARTESIAN_MOTORS, ROBOT_SPHERICAL_MOTORS))
return
interp = Array(move(simulate=True, coordinates = k, **kwargs)).T.interpolate(10).T
if k == "spherical":
interp = Array([robot.cart2sph(return_dict=False, **{k:e for k,e in zip(ROBOT_CARTESIAN_MOTORS,v)}) for v in move(simulate=True, coordinates = "cartesian", **kwargs)]).T.interpolate(10).T
else:
interp = Array(move(simulate=True, coordinates = k, **kwargs)).T.interpolate(10).T
self.last_remote_motion = "recording"
move(sync = True, **kwargs)
frame = Array(self.frame_trsf)
tool = Array(self.tool_trsf)
self.remote_allowed[k]["frame"].append(frame)
self.remote_allowed[k]["tool"].append(tool)
self.remote_allowed[k]["pos"].append(interp)
self.remote_allowed_recorded[k]["frame"].append(frame)
self.remote_allowed_recorded[k]["tool"].append(tool)
self.remote_allowed_recorded[k]["pos"].append(interp)
self.on_recording_finished()
return True
@@ -327,11 +349,12 @@ class RobotBernina(RobotTCP):
x = r*math.cos(delta)*math.sin(gamma)
y = r*math.sin(delta)
z = r*math.cos(delta)*math.cos(gamma)
rx, ry = self.rad2deg([-delta,+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 return_dict:
ret = {"x": x, "y": y, "z": z, "rx": rx, "ry": ry}
ret = {"x": x, "y": y, "z": z, "rx": rx, "ry": ry, "rz":rz}
else:
ret = [x, y, z, rx, ry]
ret = [x, y, z, rx, ry, rz]
return ret
def cart2sph(self, x=None, y=None, z=None, return_dict=True, **kwargs):
@@ -412,7 +435,7 @@ class RobotBernina(RobotTCP):
if v == None:
target_cart[k] = current_cart[k]
if (self.working_mode == "remote") & (~simulate):
if not remote_allowed(current_cart, target_cart, frame_trsf=self.get_trsf(frame+".trsf"), tool_trsf=self.get_trsf(tool+".trsf"), motion="cartesian"):
if not self.remote_allowed(current_cart, target_cart, frame_trsf=self.get_trsf(frame+".trsf"), tool_trsf=self.get_trsf(tool+".trsf"), motion="cartesian"):
get_context().sendEvent("motion", "Requested remote motion is not in recorded trajectories. Use rob.record_motion(*args) to record the motion in manual working mode first.")
return False
self.cartesian_destination = target_cart
@@ -466,7 +489,7 @@ class RobotBernina(RobotTCP):
if v == None:
target_sph[k] = current_sph[k]
if (self.working_mode == "remote") & (~simulate):
if not remote_allowed(current_sph, target_sph, frame_trsf=self.get_trsf(frame+".trsf"), tool_trsf=self.get_trsf(tool+".trsf"), motion="spherical"):
if not self.remote_allowed(current_sph, target_sph, frame_trsf=self.get_trsf(frame+".trsf"), tool_trsf=self.get_trsf(tool+".trsf"), motion="spherical"):
get_context().sendEvent("motion", "Requested remote motion is not in recorded trajectories. Use rob.record_motion(*args) to record the motion in manual working mode first.")
return False
# First only move in the radial direction
@@ -539,7 +562,7 @@ class RobotBernina(RobotTCP):
if v == None:
target_jnt[k] = current_jnt[k]
if (self.working_mode == "remote") & (~simulate):
if not remote_allowed(current_jnt, target_jnt, frame_trsf=None, tool_trsf=None, motion="joint"):
if not self.remote_allowed(current_jnt, target_jnt, frame_trsf=None, tool_trsf=None, motion="joint"):
get_context().sendEvent("motion", "Requested remote motion is not in recorded trajectories. Use rob.record_motion(*args) to record the motion in manual working mode first.")
return False
if simulate:
@@ -585,9 +608,6 @@ class RobotBernina(RobotTCP):
#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:
script/devices/RobotTCP.py
+18 -5
View File
@@ -326,10 +326,9 @@ class RobotTCP(TcpDevice, Stoppable):
def enable(self):
if not self.is_powered():
self.evaluate("enablePower()")
time.sleep(1.0)
time.sleep(5.0)
if not self.is_powered():
raise Exception("Cannot enable power")
raise Exception("Enabling power timed out after 5s.")
#waits for power to be actually cut off
def disable(self):
self.evaluate("disablePower()", timeout=5000)
@@ -688,7 +687,13 @@ class RobotTCP(TcpDevice, Stoppable):
def _update_state(self):
#self.setState(State.Busy if self.status=="move" else State.Ready)
if self.state==State.Offline:
print "Communication resumed"
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.setPolling(DEFAULT_ROBOT_POLLING)
if self.reset or (self.state==State.Offline):
self.check_task()
if self.current_task is not None:
@@ -760,6 +765,7 @@ class RobotTCP(TcpDevice, Stoppable):
self.on_poll_status.update({
"connected": self.connected,
"powered": self.powered,
"override_remote_safety": self.override_remote_safety,
"speed": self.speed,
"settled": self.settled,
"task": self.current_task,
@@ -921,6 +927,7 @@ class RobotTCP(TcpDevice, Stoppable):
"settled": self.settled,
"task": self.current_task,
"mode": self.working_mode,
"override_remote_safety": self.override_remote_safety,
"status": self.status,
"frame": self.frame,
"tool": self.tool,
@@ -953,6 +960,7 @@ class RobotTCP(TcpDevice, Stoppable):
def on_poll_info(self):
on_poll_config_methods ={
"powered": {"cmd":"robot.set_powered", "def_kwargs": ""},
"override_remote_safety": {"cmd":"robot.set_override_remote_safety", "def_kwargs": ""},
"speed": {"cmd":"robot.set_monitor_speed", "def_kwargs": ""},
"frame": {"cmd": "robot.set_frame", "def_kwargs": "change_default=True",},
"frame_coordinates": {"cmd": "robot.set_frame_coordinates", "def_kwargs": "",},
@@ -1287,7 +1295,12 @@ class RobotTCP(TcpDevice, Stoppable):
pass
def on_change_working_mode(self, working_mode, prev_working_mode):
pass
if working_mode == "remote":
self.set_profile("remote")
get_context().sendEvent("motion", "Working mode changed from {} to {}, changing user. \n".format(prev_working_mode, working_mode))
else:
self.set_profile("default")
def on_change_status(self, status, prev_status):
if status in ["joint", "cartesian_frame", "cartesian_tool", "point"]: