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

Added target position property to motors and added complete status information of working modes

Browse Source
This commit is contained in:
gac-bernina
2024-01-12 15:02:25 +01:00
parent ac5bb50a5e
commit bf75855806
3 changed files with 209 additions and 109 deletions
Download Patch File Download Diff File Expand all files Collapse all files

56
script/devices/RobotBernina.py
View File

@@ -17,6 +17,10 @@ DEFAULT_ROBOT_POLLING = 200
TASK_WAIT_ROBOT_POLLING = 50
DEFAULT_SPEED=100
ROBOT_JOINT_MOTORS = ["j1" , "j2", "j3", "j4", "j5", "j6"]
ROBOT_CARTESIAN_MOTORS = ["x" , "y", "z", "rx", "ry", "rz"]
ROBOT_SPHERICAL_MOTORS = ["r" , "gamma", "delta"]
run("devices/RobotTCP")
@@ -32,6 +36,19 @@ class RobotBernina(RobotTCP):
self.last_command_timestamp = None
self.last_command_position = None
#self.setSimulated() # TODO: Remove me
def set_motion_queue_empty(self, state=False):
### update the status of the robot, the queue of motion commands
### the status is polled automatically from the controller but this setting command does not have a delay
self.empty = state
self._update_state()
def check_software_limits(self, j1=0, j2=0, j3=0, j4=0, j5=0, j6=0):
j = [j1,j2,j3,j4,j5,j6]
self.set_jnt(j, name="tcp_j")
self.evaluate("tcp_b=isInRange(tcp_j)")
return self.get_bool(name="tcp_b")
def deg2rad(self, angles):
return [a/180.*math.pi for a in angles]
@@ -46,7 +63,6 @@ class RobotBernina(RobotTCP):
def update_spherical_pos(self):
self.spherical_pos=self.cart2sph(return_dict=True, **self.cartesian_pos)
def get_movel_interpolation(self, i, coordinates="joint", idx_pnt1=0, idx_pnt2=1):
"""
coordinates: "joint" or "cartesian"
@@ -155,6 +171,7 @@ class RobotBernina(RobotTCP):
for i in range(11):
sim.append(self.get_movel_interpolation(i/10., coordinates=coordinates))
else:
self.set_motion_queue_empty(False)
ret = self.movel("tcp_p_spherical[1]", tool=tool, desc=None, sync=sync)
if simulate:
return sim
@@ -207,6 +224,7 @@ class RobotBernina(RobotTCP):
for i in range(11):
sim.append(self.get_movel_interpolation(i/10., coordinates=coordinates))
else:
self.set_motion_queue_empty(False)
ret = self.movel("tcp_p_spherical[1]", tool=tool, desc=None, sync=sync)
print("moving radius")
@@ -227,21 +245,45 @@ class RobotBernina(RobotTCP):
if (self.point_reachable("tcp_p_spherical[2]"))&(self.point_reachable("tcp_p_spherical[3]"))&(self.distance_p("tcp_p_spherical[1]", "tcp_p_spherical[3]")>.1):
if self.distance_p("tcp_p_spherical[2]", "tcp_p_spherical[3]")>1:
if simulate:
for i in range(10):
sim.append(self.get_movec_interpolation(i/10.+0.1, coordinates=coordinates))
if len(sim)==0:
for i in range(11):
sim.append(self.get_movec_interpolation(i/10., coordinates=coordinates))
else:
for i in range(10):
sim.append(self.get_movec_interpolation(i/10.+0.1, coordinates=coordinates))
else:
ret = self.movec("tcp_p_spherical[2]", "tcp_p_spherical[3]", tool=None, desc=None, sync=sync)
print("moving angle circle")
else:
if simulate:
for i in range(10):
sim.append(self.get_movel_interpolation(i/10.+0.1, coordinates=coordinates, idx_pnt1=1, idx_pnt2=3))
if len(sim==0):
for i in range(11):
sim.append(self.get_movel_interpolation(i/10., coordinates=coordinates, idx_pnt1=1, idx_pnt2=3))
else:
for i in range(10):
sim.append(self.get_movel_interpolation(i/10.+0.1, coordinates=coordinates, idx_pnt1=1, idx_pnt2=3))
else:
self.set_motion_queue_empty(False)
ret = self.movel("tcp_p_spherical[3]", tool=None, desc=None, sync=sync)
print("moving angle linear")
if simulate:
return sim
def move_joint(self, j1=None, j2=None, j3=None, j4=None, j5=None, j6=None, tool=None, desc=None, sync=False, simulate=False, coordinates="joint"):
# Simulation not implemented yet
if desc == None:
desc = self.default_desc
if tool == None:
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():
if v == None:
target_jnt[k] = current_jnt[k]
self.robot.set_jnt(target_jnt, name="tcp_j")
self.set_motion_queue_empty(False)
self.robot.movej("tcp_j", tool , desc)
def move_home(self):
if not self.is_in_point(P_HOME):
self.start_task(MOVE_HOME)
@@ -348,5 +390,5 @@ 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.motor_to_epics_pvs(robot.cartesian_motors+robot.spherical_motors+robot.joint_motors)
robot.motor_to_epics_pvs(robot.cartesian_motors+robot.spherical_motors+robot.joint_motors+robot.linear_axis_motors)
#show_panel(robot)

70
script/devices/RobotMotors.py
View File

@@ -6,8 +6,13 @@ class LinearAxisMotor(PositionerBase):
PositionerBase.__init__(self, name, PositionerConfig())
self.robot = robot
@property
def target_coordinates(self):
return {k: m.setpoint for k,m in robot.linear_axis_motors.items()}
#ATTENTION: Always initialize linear axis motors before scanning (or call robot.set_lin_axis_motor_enabled(True))
def doInitialize(self):
self.setpoint = self.doReadReadback()
self.setCache(self.doRead(), None)
def doRead(self):
@@ -15,29 +20,32 @@ class LinearAxisMotor(PositionerBase):
def doWrite(self, value):
if self.robot.linear_axis_destination is not None:
self.setpoint = value
self.robot.linear_axis_destination.update({self.name: value})
self.setCache(float(value), None)
robot.evaluate("n_moveAbsPos = " + str(value))
robot.evaluate("setEcAo(ecao_posSet,n_moveAbsPos)")
def doReadReadback(self):
return float("nan") if self.robot.linear_axis_pos is None else float(self.robot.linear_axis_pos[self.name])
return float("nan") if len(self.robot.linear_axis_pos)==0 else float(self.robot.linear_axis_pos[self.name])
def stop(self):
robot.evaluate("setEcDo(ecdo_linMotStop,true)")
time.sleep(0.1)
robot.evaluate("setEcDo(ecdo_linMotStop,false)")
ROBOT_MOTORS = ["x" , "y", "z", "rx", "ry", "rz"]
class RobotCartesianMotor(PositionerBase):
def __init__(self, robot, index):
PositionerBase.__init__(self, ROBOT_MOTORS[index], PositionerConfig())
self.index = index
def __init__(self, robot, name):
PositionerBase.__init__(self, name, PositionerConfig())
self.robot = robot
@property
def target_coordinates(self):
return {k: m.setpoint for k,m in robot.cartesian_motors.items()}
#ATTENTION: Always initialize cartesian motors before scanning (or call robot.set_motors_enabled(True))
def doInitialize(self):
self.setpoint = self.doReadReadback()
self.setCache(self.doRead(), None)
def doRead(self):
@@ -45,11 +53,12 @@ class RobotCartesianMotor(PositionerBase):
def doWrite(self, value):
if self.robot.cartesian_destination is not None:
self.setpoint = value
self.setCache(float(value), None)
robot.move_cartesian(**{self.name:value})
def doReadReadback(self):
return float("nan") if self.robot.cartesian_pos is None else float(self.robot.cartesian_pos[self.name])
return float("nan") if len(self.robot.cartesian_pos)==0 else float(self.robot.cartesian_pos[self.name])
def stop(self):
robot.stop()
@@ -59,40 +68,46 @@ class RobotCartesianMotor(PositionerBase):
def move_done(self):
"""once move done, do readback update and send event and readback"""
ROBOT_SPHERICAL_MOTORS = ["r" , "gamma", "delta"]
class RobotSphericalMotor (PositionerBase):
def __init__(self, robot, index):
PositionerBase.__init__(self, ROBOT_SPHERICAL_MOTORS[index], PositionerConfig())
self.index = index
def __init__(self, robot, name):
PositionerBase.__init__(self, name, PositionerConfig())
self.robot = robot
@property
def target_coordinates(self):
return {k: m.setpoint for k,m in robot.spherical_motors.items()}
def doInitialize(self):
self.setpoint = self.doReadReadback()
self.setCache(self.doRead(), None)
def doRead(self):
return float("nan") if self.robot.spherical_pos is None else robot.spherical_pos[self.name]
return float("nan") if len(self.robot.spherical_pos)==0 else robot.spherical_pos[self.name]
def doWrite(self, value):
if self.robot.spherical_destination is not None:
self.setpoint = value
self.setCache(float(value), None)
robot.move_spherical(**{self.name:value})
def doReadReadback(self):
return float("nan") if self.robot.spherical_pos is None else robot.spherical_pos[self.name]
return float("nan") if len(self.robot.spherical_pos)==0 else robot.spherical_pos[self.name]
def stop(self):
robot.stop()
robot.reset_motion()
robot.resume()
ROBOT_JOINT_MOTORS = ["j1" , "j2", "j3", "j4", "j5", "j6"]
class RobotJointMotor (PositionerBase):
def __init__(self, robot, index):
PositionerBase.__init__(self, ROBOT_JOINT_MOTORS[index], PositionerConfig())
self.index = index
self.robot = robot
def __init__(self, robot, name):
PositionerBase.__init__(self, name, PositionerConfig())
self.robot = robot
@property
def target_coordinates(self):
return {k: m.setpoint for k,m in robot.joint_motors.items()}
def doInitialize(self):
self.setpoint = self.doReadReadback()
self.setCache(self.doRead(), None)
@@ -100,16 +115,13 @@ class RobotJointMotor (PositionerBase):
def doRead(self):
return self.setpoint
def doWrite(self, value):
#print "Move " + ROBOT_JOINT_MOTORS[self.index] + " to " + str(value)
def doWrite(self, 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_DEFAULT)
self.robot.move_joint(**{self.name:value})
def doReadReadback(self):
return self.robot.herej()[self.index] if self.robot.joint_pos is None else float(self.robot.joint_pos[self.name])
return self.robot.get_joint_pos()[self.name] if len(self.robot.joint_pos)==0 else float(self.robot.joint_pos[self.name])
def stop(self):
robot.stop()

192
script/devices/RobotTCP.py
View File

@@ -39,13 +39,13 @@ class RobotTCP(TcpDevice, Stoppable):
self.joint_pos = {}
self.linear_axis_pos = {}
self.cartesian_motors_enabled = False
self.cartesian_motors = []
self.cartesian_motors = {}
self.spherical_motors_enabled = False
self.spherical_motors = []
self.spherical_motors = {}
self.linear_axis_motors_enabled = None
self.linear_axis_motors = []
self.linear_axis_motors = {}
self.joint_motors_enabled = False
self.joint_motors = []
self.joint_motors = {}
self.tool = None
self.default_desc = None
self.tool_open = None
@@ -354,19 +354,50 @@ class RobotTCP(TcpDevice, Stoppable):
if str(ret) != "0": raise Exception("Error saving program: " + str(ret))
def _update_working_mode(self, mode, status):
modestr = {
1: {
"mode": "manual",
"status": {
0: "programmed",
1: "connection",
2: "joint",
3: "cartesian_frame",
4: "cartesian_tool",
5: "point",
6: "hold",
},
},
2: {
"mode": "test",
"status": {
0: "programmed",
1: "connection",
2: "programmed_fast",
3: "hold",
},
},
3: {
"mode": "local",
"status": {
0: "programmed",
1: "connection",
2: "hold",
},
},
4: {
"mode": "remote",
"status": {
0: "programmed",
1: "connection",
2: "hold",
},
},
}
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"
if mode in modestr.keys():
self.working_mode = modestr[mode]["mode"]
self.status = modestr[mode]["status"][status]
else:
self.working_mode = "invalid"
self.status = "invalid"
@@ -407,12 +438,20 @@ class RobotTCP(TcpDevice, Stoppable):
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)
### reinitialize all motors (target values)
if self.cartesian_motors_enabled:
for m in self.cartesian_motors.values():
m.doInitialize()
if self.spherical_motors_enabled:
for m in self.spherical_motors.values():
m.doInitialize()
if self.linear_axis_motors_enabled:
for m in self.linear_axis_motors.values():
m.doInitialize()
if self.joint_motors_enabled:
for m in self.joint_motors.values():
m.doInitialize()
def is_empty(self):
self.empty = self.eval_bool("isEmpty()")
@@ -742,12 +781,12 @@ class RobotTCP(TcpDevice, Stoppable):
else:
sts = self.execute("get_status_fast", cur_task)
for i in range(6):
self.joint_pos[ROBOT_JOINT_MOTORS[i]] = float(sts[0 + i])
for i in range(6):
self.cartesian_pos[ROBOT_MOTORS[i]] = float(sts[6 + i])
for i, name in enumerate(ROBOT_JOINT_MOTORS):
self.joint_pos[name] = float(sts[0 + i])
for i, name in enumerate(ROBOT_CARTESIAN_MOTORS):
self.cartesian_pos[name] = float(sts[6 + i])
self.linear_axis_pos = {"z_lin": float(sts[7])}
self.linear_axis_pos = {"z_lin": float(sts[12])}
self.update_spherical_pos()
ev_index = 8 #7
@@ -767,19 +806,20 @@ class RobotTCP(TcpDevice, Stoppable):
self.on_poll_status.update({
"pos": pos,
})
if self.cartesian_motors_enabled:
for m in self.cartesian_motors:
for m in self.cartesian_motors.values():
m.readback.update()
if self.spherical_motors_enabled:
for m in self.spherical_motors:
for m in self.spherical_motors.values():
m.readback.update()
if self.linear_axis_motors_enabled:
for m in self.linear_axis_motors:
for m in self.linear_axis_motors.values():
m.readback.update()
if self.joint_motors_enabled:
for m in self.joint_motors:
for m in self.joint_motors.values():
m.readback.update()
def doUpdate(self):
try:
@@ -884,16 +924,16 @@ class RobotTCP(TcpDevice, Stoppable):
get_context().sendEvent("polling", self.on_poll_status)
if self.cartesian_motors_enabled:
for m in self.cartesian_motors:
for m in self.cartesian_motors.values():
m.readback.update()
if self.spherical_motors_enabled:
for m in self.spherical_motors:
for m in self.spherical_motors.values():
m.readback.update()
if self.linear_axis_motors_enabled:
for m in self.linear_axis_motors:
for m in self.linear_axis_motors.values():
m.readback.update()
if self.joint_motors_enabled:
for m in self.joint_motors:
for m in self.joint_motors.values():
m.readback.update()
@@ -908,27 +948,30 @@ class RobotTCP(TcpDevice, Stoppable):
"powered": {"cmd":"robot.set_powered", "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_trsf", "def_kwargs": "name='f_actualFrame.trsf'",},
"frame_coordinates": {"cmd": "robot.set_frame_coordinates", "def_kwargs": "",},
"tool": {"cmd":"robot.set_tool", "def_kwargs": ""},
"tool_coordinates": {"cmd": "robot.set_trsf", "def_kwargs": "name='t_actualTool.trsf'",},
"tool_coordinates": {"cmd": "robot.set_tool_coordinates", "def_kwargs": "",},
"cartesian_motors_enabled": {"cmd":"robot.set_motors_enabled", "def_kwargs": "pseudos=['cartesian']"},
"spherical_motors_enabled": {"cmd":"robot.set_motors_enabled", "def_kwargs": "pseudos=['spherical']"},
}
on_poll_info = [k for k in self.on_poll_status if not k in list(on_poll_config_methods.keys())+["pos"]]
return {"info": on_poll_info, "config": on_poll_config_methods}
def set_tool_coordinates(self, v, name=None):
if name is None:
name = self.tool
self.set_trsf(v, name=name + ".trsf")
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
self.set_trsf(v, name=name + ".trsf")
if name == self.frame:
self.set_trsf(v, name="f_actualFrame.trsf")
#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, return_dict=True):
if tool is None:
self.assert_tool()
@@ -945,11 +988,14 @@ class RobotTCP(TcpDevice, Stoppable):
else:
ret = [x, y, z, rx, ry, rz]
return ret
#a = self.execute('get_pos', tool, frame)
#ret = []
#for i in range(6): ret.append(float(a[i]))
#return ret
def get_joint_pos(self, return_dict=True):
js = self.herej()
if return_dict:
ret = {k: v for k, v in zip(["j1", "j2", "j3", "j4", "j5", "j6"], js)}
else:
ret = js
return ret
def get_flange_pos(self, frame=None, return_dict=True):
return self.get_cartesian_pos(FLANGE, frame, return_dict=return_dict)
@@ -1045,75 +1091,75 @@ class RobotTCP(TcpDevice, Stoppable):
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))
for i in ROBOT_CARTESIAN_MOTORS:
self.cartesian_motors[i] = RobotCartesianMotor(self, i)
add_device(self.cartesian_motors[i], True)
self.cartesian_destination = self.get_cartesian_pos(return_dict=True)
self.cartesian_pos = self.cartesian_destination
else:
for m in self.cartesian_motors:
for m in self.cartesian_motors.values():
remove_device(m)
self.cartesian_motors = []
self.cartesian_motors = {}
self.cartesian_destination = None
else:
if value:
self.cartesian_destination = self.get_cartesian_pos(return_dict=True)
for m in self.cartesian_motors:
for m in self.cartesian_motors.values():
m.initialize()
if "spherical" in pseudos:
if value !=self.spherical_motors_enabled:
self.spherical_motors_enabled = value
if value:
for i in range(3):
self.spherical_motors.append(RobotSphericalMotor(self, i))
add_device(self.spherical_motors[i], True)
for i in ROBOT_SPHERICAL_MOTORS:
self.spherical_motors[i] = RobotSphericalMotor(self, i)
add_device(self.spherical_motors[i], True)
self.spherical_destination = self.get_spherical_pos()
self.spherical_pos = self.spherical_destination
else:
for m in self.spherical_motors:
for m in self.spherical_motors.values():
remove_device(m)
self.spherical_motors = []
self.spherical_motors = {}
self.spherical_destination = None
else:
if value:
self.spherical_destination = self.get_spherical_pos()
for m in self.spherical_motors:
for m in self.spherical_motors.values():
m.initialize()
if "linear" in pseudos:
if value !=self.linear_axis_motors_enabled:
self.linear_axis_motors_enabled = value
if value:
self.linear_axis_motors.append(LinearAxisMotor(self, "z_lin"))
add_device(self.linear_axis_motors[0], True)
self.linear_axis_motors["z_lin"] = LinearAxisMotor(self, "z_lin")
add_device(self.linear_axis_motors["z_lin"], True)
self.linear_axis_destination = self.get_linear_axis_pos()
self.linear_axis_pos = self.linear_axis_destination
else:
for m in self.linear_axis_motors:
for m in self.linear_axis_motors.values():
remove_device(m)
self.linear_axis_motors = []
self.linear_axis_destination = None
else:
if value:
self.linear_axis_destination = self.get_linear_axis_pos()
for m in self.linear_axis_motors:
for m in self.linear_axis_motors.values():
m.initialize()
#Joint 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)
for i in ROBOT_JOINT_MOTORS:
self.joint_motors[i] = RobotJointMotor(self, i)
add_device(self.joint_motors[i], True)
else:
for m in self.joint_motors:
for m in self.joint_motors.values():
remove_device(m)
self.joint_motors = []
self.joint_motors = {}
else:
if value:
for m in self.joint_motors:
for m in self.joint_motors.values():
m.initialize()
#Alignment
def align(self, point = None, frame = None, desc = None):