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

Fixed problems with gamma < -90 and nuerical issues when delta --> 90

Browse Source
This commit is contained in:
sander_m
2025-05-02 09:54:55 +02:00
parent 9af3d992d4
commit 06283c1ec5
3 changed files with 60 additions and 140 deletions
Download Patch File Download Diff File Expand all files Collapse all files

3
config/config.properties
View File

@@ -1,4 +1,4 @@
#Tue Aug 27 08:44:59 CEST 2024
#Thu Jan 16 15:06:14 CET 2025
xscanMoveTimeout=600
autoSaveScanData=true
simulation=false
@@ -39,6 +39,7 @@ filePermissionsLogs=Public
logPath={logs}/{date}_{time}
dataLayout=default
disableDataFileLogs=false
dataScanSaveTimestamps=false
sessionHandling=Off
terminalEnabled=true
filePermissionsScripts=Public

52
script/devices/RobotBernina.py
View File

@@ -9,6 +9,7 @@ DESC_DEFAULT = DESCS[0]
FLANGE = "flange"
#DEFAULT_ROBOT_POLLING = 200
DEFAULT_ROBOT_POLLING = 200
TASK_WAIT_ROBOT_POLLING = 50
DEFAULT_SPEED=100
@@ -222,19 +223,26 @@ class RobotBernina(RobotTCP):
# New try first delta
ry = -math.asin(-math.cos(delta)*math.sin(gamma))
rx = -math.acos(math.cos(delta)*math.cos(gamma)/math.cos(ry))
rx = (-math.acos(math.cos(delta)*math.cos(gamma)/math.cos(ry)))
rz = math.asin(-math.sin(ry)*math.sin(rx)*math.sqrt(1/(math.cos(rx)**2+(math.sin(ry)*math.sin(rx))**2)))
rx, ry, rz = self.rad2deg([rx, ry, rz])
#end new try
print rz
#Necessary to keep detector orientation for gamma larger 90
#Necessary to keep detector orientation for gamma larger 90 or smaller -90
if self.rad2deg(gamma) > 90:
rz = 180-rz
if self.rad2deg(gamma) < -90:
rz = -180-rz
if self.rad2deg(delta) > 90:
rz = -rz
#For delta --> 90, the calculation of rz runs into numerical issues:
if (self.rad2deg(delta) < 90.05)&(self.rad2deg(delta)>89.95):
rz = self.rad2deg(gamma)
if abs(rz//180) > 0:
rz = rz - (rz+180)//360*360
#Necessary correction for delta < 0
rx = math.copysign(1, delta)*rx
rz = math.copysign(1, delta)*rz
if return_dict:
ret = {"x": x, "y": y, "z": z, "rx": rx, "ry": ry, "rz":rz}
@@ -564,13 +572,32 @@ class RobotBernina(RobotTCP):
def motor_to_epics_pvs(self, motors=[]):
self.epics_pvs = {str("SARES20-ROB:" + motor.name + ".VAL").upper(): CAS(str("SARES20-ROB:" + motor.name + ".VAL").upper(), motor, "double") for motor in motors}
self.epics_pvs.update({str("SARES20-ROB:" + motor.name + ".RBV").upper(): CAS(str("SARES20-ROB:" + motor.name + ".RBV").upper(), motor.readback, "double") for motor in motors})
def on_reconnected(self):
#self.setup()
sleep(2)
get_context().restart()
#robot.latency = 0.005
def setup(self):
self.set_default_desc(DESC_DEFAULT)
self.default_speed = DEFAULT_SPEED
self.set_frame(self.frame_persistent())
self.set_frame(self.frame(), name="tcp_f_spherical", change_default=False)
self.set_tool(self.tool_persistent())
self.setPolling(DEFAULT_ROBOT_POLLING)
self.get_current_point() #TODO: REMOVE WHEN CURRENT POINT REPORTED BY POLLING MESSAGE
self.set_motors_enabled(True)
self.set_joint_motors_enabled(True)
if simulation:
add_device(RobotBernina("robot","localhost:1234"),force = True)
else:
add_device(RobotBernina("robot", "129.129.243.106:1234"), force = True)
time.sleep(0.1)
time.sleep(1)
class DummySTP(RegisterBase):
@@ -637,16 +664,9 @@ def make_archivable(Motor):
epics_pvs.update({base + ".FLNK": CAS((base+".FLNK").upper(), DummySTP((base+".RBV").upper()), "string")})
Motor.epics_pvs = epics_pvs
#robot.latency = 0.005
robot.set_default_desc(DESC_DEFAULT)
robot.default_speed = DEFAULT_SPEED
robot.set_frame(robot.frame_persistent())
robot.set_frame(robot.frame(), name="tcp_f_spherical", change_default=False)
robot.set_tool(robot.tool_persistent())
robot.setPolling(DEFAULT_ROBOT_POLLING)
robot.get_current_point() #TODO: REMOVE WHEN CURRENT POINT REPORTED BY POLLING MESSAGE
robot.set_motors_enabled(True)
robot.set_joint_motors_enabled(True)
robot.setup()
motors = list(robot.cartesian_motors.values())+list(robot.spherical_motors.values())+list(robot.joint_motors.values())+list(robot.linear_axis_motors.values())
for m in motors:
make_archivable(m)

145
script/devices/RobotTCP.py
View File

@@ -496,7 +496,7 @@ class RobotTCP(TcpDevice, Stoppable):
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")
self.set_pnt(joint_or_point , "tcp_p")
joint_or_point = "tcp_p"
#TODO: in new robot movel and movej is freezing controller
@@ -515,7 +515,7 @@ class RobotTCP(TcpDevice, Stoppable):
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")
self.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 +")")
@@ -579,7 +579,7 @@ class RobotTCP(TcpDevice, Stoppable):
def is_tool_open(self, tool=None):
if tool is None: tool = self.tool()
self.tool_open = robot.eval_bool(tool + ".gripper")
self.tool_open = self.eval_bool(tool + ".gripper")
return self.tool_open
@@ -688,18 +688,15 @@ class RobotTCP(TcpDevice, Stoppable):
except:
ret[i] = None
return ret
def on_reconnected(self):
pass
def _update_state(self):
#self.setState(State.Busy if self.status=="move" else State.Ready)
if self.state==State.Offline:
print "Communication resumed"
sleep(5)
robot.set_default_desc(DESC_DEFAULT)
robot.default_speed = DEFAULT_SPEED
robot.set_frame(self.frame_persistent())
robot.set_frame(self.frame_persistent(), name="tcp_f_spherical", change_default=False)
robot.set_tool(self.tool_persistent())
robot.setPolling(DEFAULT_ROBOT_POLLING)
self.on_reconnected()
if self.reset or (self.state==State.Offline):
self.check_task()
if self.current_task is not None:
@@ -758,7 +755,7 @@ class RobotTCP(TcpDevice, Stoppable):
self._update_state()
self.reset = False
self.setCache({"powered": self.powered,
self.updateCache({"powered": self.powered,
"speed": self.speed,
"empty": self.empty,
"settled": self.settled,
@@ -766,7 +763,7 @@ class RobotTCP(TcpDevice, Stoppable):
"mode": self.working_mode,
"status": self.status,
"open": self.tool_open,
}, None)
})
self.on_poll_status.update({
"connected": self.connected,
@@ -787,7 +784,12 @@ class RobotTCP(TcpDevice, Stoppable):
def updateCache(self, dictionary):
cur = self.take()
if cur is None:
cur={}
cur.update(dictionary)
self.setCache(cur, None)
def doUpdate_fast(self):
@@ -807,16 +809,17 @@ class RobotTCP(TcpDevice, Stoppable):
self.linear_axis_pos = {"z_lin": float(sts[12])}
self.update_spherical_pos()
"""
ev_index = 8 #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.setCache({
# "pos": self.get_current_point_cached() if self.state==State.Ready else None #TODO: make it calculated in robot by polling funtion
# }, None)
"""
self.updateCache({
"pos": self.get_current_point_cached() if self.state==State.Ready else None #TODO: make it calculated in robot by polling funtion
})
pos = self.cartesian_pos.copy()
pos.update(self.spherical_pos)
pos.update(self.joint_pos)
@@ -858,110 +861,6 @@ class RobotTCP(TcpDevice, Stoppable):
self.setState(State.Offline)
self.on_polling()
def doUpdate_backup(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[ROBOT_JOINT_MOTORS[i]] = float(sts[8 + i])
for i in range(6):
self.cartesian_pos[ROBOT_MOTORS[i]] = float(sts[14 + i])
self.linear_axis_pos = {"z_lin": float(sts[20])}
self.update_spherical_pos()
ev_index = 21 #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)
pos = self.cartesian_pos.copy()
pos.update(self.spherical_pos)
pos.update(self.joint_pos)
pos.update(self.linear_axis_pos)
self.on_poll_status = {
"connected": self.connected,
"powered": self.powered,
"speed": self.speed,
"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(),
"pos": pos,
"cartesian_motors_enabled": self.cartesian_motors_enabled,
"spherical_motors_enabled": self.spherical_motors_enabled,
}
get_context().sendEvent("polling", self.on_poll_status)
if self.cartesian_motors_enabled:
for m in self.cartesian_motors.values():
m.readback.update()
if self.spherical_motors_enabled:
for m in self.spherical_motors.values():
m.readback.update()
if self.linear_axis_motors_enabled:
for m in self.linear_axis_motors.values():
m.readback.update()
if self.joint_motors_enabled:
for m in self.joint_motors.values():
m.readback.update()
except:
if self.state != State.Offline:
print >> sys.stderr, "Update error: " + str(sys.exc_info()[1])
self.setState(State.Offline)
def on_poll_info(self):
on_poll_config_methods ={
@@ -1104,7 +1003,7 @@ class RobotTCP(TcpDevice, Stoppable):
raise Exception ("Not in position " + p)
def is_emulation(self):
return "localhost" in robot.client.getServerAddress()
return "localhost" in self.client.getServerAddress()
#Cartesian peudo-motors
def set_motors_enabled(self, value, pseudos=["cartesian", "spherical", "linear"]):