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

Added spherical & cartesian coordinates. First test of linear axis.

Browse Source
This commit is contained in:
gac-bernina
2023-11-15 13:52:49 +01:00
parent 21be0ef7cc
commit 2e8a8e70ac
4 changed files with 172 additions and 54 deletions
Download Patch File Download Diff File Expand all files Collapse all files

11
devices/z_lin.properties Normal file
View File

@@ -0,0 +1,11 @@
#Tue Nov 14 14:31:51 CET 2023
description=null
maxValue=NaN
minValue=NaN
offset=0.0
precision=-1
resolution=NaN
rotation=false
scale=1.0
sign_bit=0
unit=null

138
script/devices/RobotBernina.py
View File

@@ -46,34 +46,33 @@ class RobotBernina(RobotTCP):
def update_spherical_pos(self):
self.spherical_pos=self.cart2sph(return_dict=True, **self.cartesian_pos)
def get_movec_interpolated(self, r=None, gamma=None, delta=None, tool=None, frame=None):
if frame == None:
frame = self.frame
if tool == None:
tool = self.tool
self.set_frame(frame, name="tcp_f_spherical", change_default=False)
current_cart = self.get_cartesian_pos(frame = frame, return_dict=True)
current_sph = self.get_spherical_pos(frame = frame, return_dict=True)
target_sph = {"r": r, "gamma": gamma, "delta": delta}
for k, v in target_sph.items():
if v == None:
target_sph[k] = current_sph[k]
self.set_pnt([current_cart[k] for k in ["x", "y", "z", "rx", "ry", "rz"]], name="tcp_p_spherical[0]")
target_cart = current_cart.copy()
target_cart.update(self.sph2cart(return_dict=True, **target_sph))
self.set_pnt([target_cart[k] for k in ["x", "y", "z", "rx", "ry", "rz"]], name="tcp_p_spherical[3]")
intermediate_cart = current_cart.copy()
intermediate_sph = target_sph.copy()
intermediate_sph.update({
"gamma": (target_sph["gamma"]+current_sph["gamma"])/2,
"delta": (target_sph["delta"]+current_sph["delta"])/2
})
intermediate_cart.update(self.sph2cart(return_dict=True, **intermediate_sph))
self.set_pnt([intermediate_cart[k] for k in ["x", "y", "z", "rx", "ry", "rz"]], name="tcp_p_spherical[2]")
a = self.execute("get_movec_interpolation")
return a
def get_movel_interpolation(self, i, coordinates="joint", idx_pnt1=0, idx_pnt2=1):
"""
coordinates: "joint" or "cartesian"
returns the interpolated position in joint or cartesian coordinates at fraction i
of the linear motion from point tcp_p_spherical[idx_pnt1] to tcp_p_spherical[idx_pnt2].
Note: i=0 returns the start point, i=1 the end point of the motion
"""
a = self.execute("get_movel_interpolation", i, coordinates, idx_pnt1, idx_pnt2)
ret = []
for i in range(6): ret.append(float(a[i]))
return ret
def get_movec_interpolation(self, i, coordinates="joint"):
"""
coordinates: "joint" or "cartesian"
returns the interpolated position in joint or cartesian coordinates at fraction i
of the circular motion along the circle given by the start point tcp_p_spherical[1],
the intermediate point tcp_p_spherical[2] and the target tcp_p_spherical[3].
Note: i=0 returns the start point, i=1 the end point of the motion
"""
a = self.execute("get_movec_interpolation", i, coordinates)
ret = []
for i in range(6): ret.append(float(a[i]))
return ret
def sph2cart(self, r=None, gamma=None, delta=None, return_dict=True, **kwargs):
gamma, delta = self.deg2rad([gamma, delta])
@@ -119,7 +118,23 @@ class RobotBernina(RobotTCP):
print("Point is not in range")
return reachable
def move_cartesian(self, x=None, y=None, z=None, rx=None, ry=None, rz=None, tool=None, frame=None, desc=None, sync=False):
def move_cartesian(self, x=None, y=None, z=None, rx=None, ry=None, rz=None, tool=None, frame=None, desc=None, sync=False, simulate=False, coordinates="joint"):
"""
Motion in the cartesian coordinate system using a linear motion movel command to
move from point tcp_p_spherical[0] to tcp_p_spherical[1].
frame: string
The frame defines the cartesian coordinate system and origin. Must exist on the controller.
sync: True or False
If true, no further commands are accepted until the motion is finished.
simulate: True or False
coordinates: "joint" or "cartesian"
If simulate = True, an array of interpolated positions in either joint or cartesian
coordinates is returned. Setting coordinates only has an effect, when the motion is simulated.
"""
sim = []
if frame == None:
frame = self.frame
if tool == None:
@@ -136,11 +151,34 @@ class RobotBernina(RobotTCP):
self.set_pnt([current_cart[k] for k in ["x", "y", "z", "rx", "ry", "rz"]], name="tcp_p_spherical[0]")
self.set_pnt([target_cart[k] for k in ["x", "y", "z", "rx", "ry", "rz"]], name="tcp_p_spherical[1]")
if self.point_reachable("tcp_p_spherical[1]", verbose=True):
ret = self.movel("tcp_p_spherical[1]", tool=tool, desc=None, sync=sync)
print(ret)
if simulate:
for i in range(11):
sim.append(self.get_movel_interpolation(i/10., coordinates=coordinates))
else:
ret = self.movel("tcp_p_spherical[1]", tool=tool, desc=None, sync=sync)
if simulate:
return sim
def move_spherical(self, r=None, gamma=None, delta=None, tool=None, frame=None, desc=None, sync=False):
def move_spherical(self, r=None, gamma=None, delta=None, tool=None, frame=None, desc=None, sync=False, simulate=False, coordinates="joint"):
"""
Motion in the spherical coordinate system using a linear moteion movel command to
change the radius from point tcp_p_spherical[0] to tcp_p_spherical[1], followed
by a circular motion along the circle given by the start point tcp_p_spherical[1],
the intermediate point tcp_p_spherical[2] and the target tcp_p_spherical[3].
frame: string
The frame defines the cartesian coordinate system and origin. Must exist on the controller.
sync: True or False
If true, no further commands are accepted until the motion is finished.
simulate: True or False
coordinates: "joint" or "cartesian"
If simulate = True, an array of interpolated positions in either joint or cartesian
coordinates is returned. Setting coordinates only has an effect, when the motion is simulated.
"""
sim = []
if frame == None:
frame = self.frame
if tool == None:
@@ -155,8 +193,6 @@ class RobotBernina(RobotTCP):
for k, v in target_sph.items():
if v == None:
target_sph[k] = current_sph[k]
#def movel(self, point, tool=None, desc=None, sync=False)
#def movec(self, point_interm, point_target, tool=None, desc=None, sync=False)
# First only move in the radial direction
target_sph_linear = current_sph.copy()
@@ -167,9 +203,12 @@ class RobotBernina(RobotTCP):
self.set_pnt([current_cart[k] for k in ["x", "y", "z", "rx", "ry", "rz"]], name="tcp_p_spherical[0]")
self.set_pnt([target_cart_linear[k] for k in ["x", "y", "z", "rx", "ry", "rz"]], name="tcp_p_spherical[1]")
if (self.point_reachable("tcp_p_spherical[1]"))&(self.distance_p("tcp_p_spherical[0]", "tcp_p_spherical[1]")>.1):
ret = self.movel("tcp_p_spherical[1]", tool=tool, desc=None, sync=sync)
print(ret)
print("moving radius")
if simulate:
for i in range(11):
sim.append(self.get_movel_interpolation(i/10., coordinates=coordinates))
else:
ret = self.movel("tcp_p_spherical[1]", tool=tool, desc=None, sync=sync)
print("moving radius")
# Next move angles
target_cart = current_cart.copy()
@@ -187,12 +226,21 @@ class RobotBernina(RobotTCP):
self.set_pnt([intermediate_cart[k] for k in ["x", "y", "z", "rx", "ry", "rz"]], name="tcp_p_spherical[2]")
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:
ret = self.movec("tcp_p_spherical[2]", "tcp_p_spherical[3]", tool=None, desc=None, sync=sync)
print("moving angle circle")
if simulate:
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:
ret = self.movel("tcp_p_spherical[3]", tool=None, desc=None, sync=sync)
print("moving angle linear")
print(ret)
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))
else:
ret = self.movel("tcp_p_spherical[3]", tool=None, desc=None, sync=sync)
print("moving angle linear")
if simulate:
return sim
def move_home(self):
if not self.is_in_point(P_HOME):
@@ -278,13 +326,15 @@ class RobotBernina(RobotTCP):
def wait_ready(self):
self.waitState(State.Ready, 1000) #robot.state.assertReady()
def motor_to_epics_pvs(self, motors=[]):
self.epics_pvs = [CAS(str("SARES20-ROB:" + motor.name + ".VAL").upper(), motor, "double") for motor in 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})
if simulation:
add_device(RobotBernina("robot","localhost:1234"),force = True)
else:
add_device(RobotBernina("robot", "129.129.243.105:1234"), force = True)
add_device(RobotBernina("robot", "129.129.243.106:1234"), force = True)
time.sleep(0.1)

34
script/devices/RobotMotors.py
View File

@@ -1,12 +1,36 @@
import time
import ch.psi.pshell.device.PositionerConfig as PositionerConfig
class LinearAxisMotor(PositionerBase):
def __init__(self, robot, name = "z_lin"):
PositionerBase.__init__(self, name, PositionerConfig())
self.robot = robot
#ATTENTION: Always initialize linear axis motors before scanning (or call robot.set_lin_axis_motor_enabled(True))
def doInitialize(self):
self.setCache(self.doRead(), None)
def doRead(self):
return float("nan") if self.robot.linear_axis_destination is None else float(self.robot.linear_axis_destination[self.name])
def doWrite(self, value):
if self.robot.linear_axis_destination is not None:
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])
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):
class RobotCartesianMotor(PositionerBase):
def __init__(self, robot, index):
PositionerBase.__init__(self, ROBOT_MOTORS[index], PositionerConfig())
self.index = index
@@ -49,7 +73,7 @@ class RobotSphericalMotor (PositionerBase):
def doRead(self):
return float("nan") if self.robot.spherical_pos is None else robot.spherical_pos[self.name]
def doWrite(self, value):
def doWrite(self, value):
if self.robot.spherical_destination is not None:
self.setCache(float(value), None)
robot.move_spherical(**{self.name:value})

43
script/devices/RobotTCP.py
View File

@@ -32,14 +32,18 @@ class RobotTCP(TcpDevice, Stoppable):
self.current_points = []
self.cartesian_destination = None
self.spherical_destination = None
self.linear_axis_destination = None
#self.flange_pos = [None] * 6
self.cartesian_pos = {}
self.spherical_pos = {}
self.joint_pos = {}
self.linear_axis_pos = {}
self.cartesian_motors_enabled = False
self.cartesian_motors = []
self.spherical_motors_enabled = False
self.spherical_motors = []
self.linear_axis_motors_enabled = None
self.linear_axis_motors = []
self.joint_motors_enabled = False
self.joint_motors = []
self.tool = None
@@ -636,7 +640,7 @@ class RobotTCP(TcpDevice, Stoppable):
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()
@@ -679,9 +683,10 @@ class RobotTCP(TcpDevice, Stoppable):
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 = 20 #7
ev_index = 21 #7
ev = sts[ev_index] if len(sts)>ev_index else ""
if len(ev.strip()) >0:
self.getLogger().info(ev)
@@ -702,6 +707,7 @@ class RobotTCP(TcpDevice, Stoppable):
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,
@@ -724,6 +730,9 @@ class RobotTCP(TcpDevice, Stoppable):
if self.spherical_motors_enabled:
for m in self.spherical_motors:
m.readback.update()
if self.linear_axis_motors_enabled:
for m in self.linear_axis_motors:
m.readback.update()
if self.joint_motors_enabled:
for m in self.joint_motors:
m.readback.update()
@@ -783,7 +792,13 @@ class RobotTCP(TcpDevice, Stoppable):
def get_flange_pos(self, frame=None, return_dict=True):
return self.get_cartesian_pos(FLANGE, frame, return_dict=return_dict)
def get_linear_axis_pos(self, return_dict = True):
val = self.get_float("n_actPosLinAx")
if return_dict:
return {"z_lin": val}
else:
return val
def get_cartesian_destination(self, tool=None, frame=None,return_dict=True):
if tool is None:
self.assert_tool()
@@ -863,7 +878,7 @@ class RobotTCP(TcpDevice, Stoppable):
return "localhost" in robot.client.getServerAddress()
#Cartesian peudo-motors
def set_motors_enabled(self, value, pseudos=["cartesian", "spherical"]):
def set_motors_enabled(self, value, pseudos=["cartesian", "spherical", "linear"]):
if "cartesian" in pseudos:
if value !=self.cartesian_motors_enabled:
self.cartesian_motors_enabled = value
@@ -903,7 +918,25 @@ class RobotTCP(TcpDevice, Stoppable):
self.spherical_destination = self.get_spherical_pos()
for m in self.spherical_motors:
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_destination = self.get_linear_axis_pos()
self.linear_axis_pos = self.linear_axis_destination
else:
for m in self.linear_axis_motors:
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:
m.initialize()
#Joint motors
def set_joint_motors_enabled(self, value):
if value !=self.joint_motors_enabled: