first bernina robot implementation
This commit is contained in:
gac-bernina
@@ -1,4 +1,4 @@
|
||||
#Mon May 15 16:26:58 CEST 2023
|
||||
#Tue Oct 31 16:26:56 CET 2023
|
||||
autoSaveScanData=true
|
||||
dataLayout=default
|
||||
dataPath={data}/{year}_{month}/{date}/{date}_{time}_{name}
|
||||
@@ -40,7 +40,7 @@ serverEnabled=true
|
||||
serverPort=8080
|
||||
sessionHandling=Off
|
||||
simulation=false
|
||||
terminalEnabled=false
|
||||
terminalEnabled=true
|
||||
terminalPort=3579
|
||||
userAuthenticator=
|
||||
userManagement=false
|
||||
|
||||
@@ -0,0 +1,11 @@
|
||||
#Tue Oct 31 17:33:12 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
|
||||
@@ -0,0 +1,11 @@
|
||||
#Tue Oct 31 17:33:12 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
|
||||
@@ -0,0 +1,11 @@
|
||||
#Thu Nov 02 10:53:37 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
|
||||
@@ -0,0 +1,11 @@
|
||||
#Thu Nov 02 10:53:37 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
|
||||
@@ -0,0 +1,11 @@
|
||||
#Thu Nov 02 10:53:37 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
|
||||
@@ -0,0 +1,11 @@
|
||||
#Thu Nov 02 10:53:37 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
|
||||
@@ -0,0 +1,11 @@
|
||||
#Thu Nov 02 10:53:37 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
|
||||
@@ -0,0 +1,11 @@
|
||||
#Thu Nov 02 10:53:37 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
|
||||
@@ -0,0 +1,11 @@
|
||||
#Tue Oct 31 17:33:12 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
|
||||
@@ -0,0 +1,11 @@
|
||||
#Thu Nov 02 10:49:38 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
|
||||
@@ -0,0 +1,11 @@
|
||||
#Thu Nov 02 10:49:38 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
|
||||
@@ -0,0 +1,11 @@
|
||||
#Thu Nov 02 10:49:38 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
|
||||
@@ -0,0 +1,11 @@
|
||||
#Thu Nov 02 10:49:38 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
|
||||
@@ -0,0 +1,11 @@
|
||||
#Thu Nov 02 10:49:38 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
|
||||
@@ -0,0 +1,11 @@
|
||||
#Thu Nov 02 10:49:38 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
|
||||
@@ -0,0 +1,11 @@
|
||||
#Mon Oct 30 14:28:29 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
|
||||
@@ -0,0 +1,11 @@
|
||||
#Mon Oct 30 14:28:29 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
|
||||
@@ -0,0 +1,11 @@
|
||||
#Mon Oct 30 14:28:29 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
|
||||
@@ -0,0 +1,11 @@
|
||||
#Thu Nov 02 13:47:11 CET 2023
|
||||
description=null
|
||||
maxValue=1000.0
|
||||
minValue=-1000.0
|
||||
offset=0.0
|
||||
precision=-1
|
||||
resolution=NaN
|
||||
rotation=false
|
||||
scale=1.0
|
||||
sign_bit=0
|
||||
unit=null
|
||||
@@ -0,0 +1,11 @@
|
||||
#Mon Oct 30 14:28:29 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
|
||||
@@ -0,0 +1,11 @@
|
||||
#Mon Oct 30 14:28:29 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
|
||||
+173
-24
@@ -1,3 +1,4 @@
|
||||
import math
|
||||
KNOWN_POINTS = P_PARK, P_HOME = "park", "home"
|
||||
TASKS = MOVE_PARK, MOVE_HOME, TWEAK_X , TWEAK_Y = "movePark", "moveHome", "tweakX", "tweakY"
|
||||
|
||||
@@ -10,9 +11,9 @@ TOOLS = ["t_Detector",]
|
||||
TOOL_DEFAULT = TOOL_DET = TOOLS[0]
|
||||
|
||||
FRAMES = ["f_actualFrame", "f_Experiment1", "f_Experiment2", "f_Experiment3"]
|
||||
FRAME_DEFAULT = FRAMES[0]
|
||||
FRAME_DEFAULT = FRAMES[1]
|
||||
|
||||
DEFAULT_ROBOT_POLLING = 500
|
||||
DEFAULT_ROBOT_POLLING = 200
|
||||
TASK_WAIT_ROBOT_POLLING = 50
|
||||
DEFAULT_SPEED=100
|
||||
|
||||
@@ -31,28 +32,167 @@ class RobotBernina(RobotTCP):
|
||||
self.last_command_timestamp = None
|
||||
self.last_command_position = None
|
||||
#self.setSimulated() # TODO: Remove me
|
||||
def deg2rad(self, angles):
|
||||
return [a/180.*math.pi for a in angles]
|
||||
|
||||
def move_spherical(self, r=None, gamma=None, delta=None):
|
||||
def rad2deg(self, angles):
|
||||
return [a*180./math.pi for a in angles]
|
||||
|
||||
def get_spherical_pos(self, frame = None, return_dict=True):
|
||||
cur_cart = self.get_cartesian_pos(frame = frame, return_dict=True)
|
||||
cur_sph = self.cart2sph(return_dict=return_dict, **cur_cart)
|
||||
return cur_sph
|
||||
|
||||
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 sph2cart(self, r=None, gamma=None, delta=None, return_dict=True, **kwargs):
|
||||
gamma, delta = self.deg2rad([gamma, delta])
|
||||
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, -math.pi/2+gamma])
|
||||
if return_dict:
|
||||
ret = {"x": x, "y": y, "z": z, "rx": rx, "ry": ry}
|
||||
else:
|
||||
ret = [x, y, z, rx, ry]
|
||||
return ret
|
||||
|
||||
def cart2sph(self, x=None, y=None, z=None, return_dict=True, **kwargs):
|
||||
r = math.sqrt(x**2 + y**2 + z**2)
|
||||
gamma = math.copysign(1, x)*math.acos(z/math.sqrt(z**2+x**2))
|
||||
delta = math.asin(y/r)
|
||||
gamma, delta = self.rad2deg([gamma, delta])
|
||||
if return_dict:
|
||||
ret = {"r": r, "gamma": gamma, "delta": delta}
|
||||
else:
|
||||
ret = [r,gamma, delta]
|
||||
return ret
|
||||
|
||||
def check_calculations(self):
|
||||
sph = self.get_spherical_pos()
|
||||
cart_calc = self.sph2cart(**sph)
|
||||
cart = self.get_cartesian_pos(return_dict=True)
|
||||
print("spherical", sph)
|
||||
print("cart_calc", cart_calc)
|
||||
print("cart", cart)
|
||||
|
||||
err = {k: cart[k]-cart_calc[k] for k in cart_calc.keys()}
|
||||
return err
|
||||
|
||||
def point_reachable(self, point,tool=None, verbose=True):
|
||||
if tool == None:
|
||||
tool = self.tool
|
||||
self.herej()
|
||||
reachable = self.eval_bool("pointToJoint(" + tool + ",tcp_j," + point + ",j)")
|
||||
if verbose:
|
||||
if not reachable:
|
||||
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):
|
||||
if frame == None:
|
||||
frame = self.frame
|
||||
if tool == None:
|
||||
tool = self.tool
|
||||
if desc == None:
|
||||
desc = self.default_desc
|
||||
self.set_frame(frame, name="tcp_f_spherical", change_default=False)
|
||||
target_cart = {"x":x, "y":y, "z":z, "rx":rx, "ry":ry, "rz":rz}
|
||||
current_cart = self.get_cartesian_pos(frame = frame, return_dict=True)
|
||||
for k, v in target_cart.items():
|
||||
if v == None:
|
||||
target_cart[k] = current_cart[k]
|
||||
self.cartesian_destination = target_cart
|
||||
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)
|
||||
|
||||
|
||||
def move_spherical(self, r=None, gamma=None, delta=None, tool=None, frame=None, desc=None, sync=False):
|
||||
if frame == None:
|
||||
frame = self.frame
|
||||
if tool == None:
|
||||
tool = self.tool
|
||||
if desc == None:
|
||||
desc = self.default_desc
|
||||
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}
|
||||
self.spherical_destination = target_sph
|
||||
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)
|
||||
|
||||
if r is not None:
|
||||
#separate in changing angle (movec) and changing r (movel)
|
||||
print("not implemented")
|
||||
else:
|
||||
x,y,z,rx,ry,rz = self.get_cartesian_pos()
|
||||
r = np.sqrt(x**2+y**2+z**2)
|
||||
z = r*np.sin(np.deg2rad(90-delta))*np.cos(np.deg2rad(gamma))
|
||||
x = r*np.sin(np.deg2rad(90-delta))*np.sin(np.deg2rad(gamma))
|
||||
y = r*np.cos(np.deg2rad(90-delta))
|
||||
ry = gamma
|
||||
rx = -delta
|
||||
# set_pnt A
|
||||
# set_pnt B
|
||||
#eval(movectl
|
||||
movec
|
||||
|
||||
|
||||
|
||||
# First only move in the radial direction
|
||||
target_sph_linear = current_sph.copy()
|
||||
target_sph_linear["r"] = target_sph["r"]
|
||||
target_cart_linear = current_cart.copy()
|
||||
tmp = self.sph2cart(return_dict=True, **target_sph_linear)
|
||||
target_cart_linear.update({k:tmp[k] for k in ["x", "y", "z"]})
|
||||
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")
|
||||
|
||||
# Next move angles
|
||||
target_cart = current_cart.copy()
|
||||
target_cart.update(self.sph2cart(return_dict=True, **target_sph))
|
||||
self.cartesian_destination = [target_cart[k] for k in ["x", "y", "z", "rx", "ry", "rz"]]
|
||||
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]")
|
||||
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")
|
||||
else:
|
||||
ret = self.movel("tcp_p_spherical[3]", tool=None, desc=None, sync=sync)
|
||||
print("moving angle linear")
|
||||
print(ret)
|
||||
|
||||
def move_home(self):
|
||||
if not self.is_in_point(P_HOME):
|
||||
@@ -70,6 +210,11 @@ class RobotBernina(RobotTCP):
|
||||
self.start_task(TWEAK_X, offset)
|
||||
self.wait_task_finished(TASK_WAIT_ROBOT_POLLING)
|
||||
|
||||
def set_powered(self, value):
|
||||
if value:
|
||||
self.enable()
|
||||
if not value:
|
||||
self.disable()
|
||||
|
||||
def tweak_y(self, offset):
|
||||
self.start_task(TWEAK_Y, offset)
|
||||
@@ -108,7 +253,7 @@ class RobotBernina(RobotTCP):
|
||||
|
||||
def set_remote_mode(self):
|
||||
self.set_profile("remote")
|
||||
|
||||
|
||||
def set_local(self):
|
||||
self.set_profile("default")
|
||||
|
||||
@@ -133,7 +278,9 @@ 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]
|
||||
|
||||
if simulation:
|
||||
add_device(RobotBernina("robot","localhost:1234"),force = True)
|
||||
else:
|
||||
@@ -148,5 +295,7 @@ robot.set_frame(FRAME_DEFAULT)
|
||||
robot.set_tool(TOOL_DEFAULT)
|
||||
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)
|
||||
#show_panel(robot)
|
||||
|
||||
@@ -8,7 +8,7 @@ ROBOT_MOTORS = ["x" , "y", "z", "rx", "ry", "rz"]
|
||||
|
||||
class RobotCartesianMotor (PositionerBase):
|
||||
def __init__(self, robot, index):
|
||||
PositionerBase.__init__(self, robot.name + "_" + ROBOT_MOTORS[index], PositionerConfig())
|
||||
PositionerBase.__init__(self, ROBOT_MOTORS[index], PositionerConfig())
|
||||
self.index = index
|
||||
self.robot = robot
|
||||
|
||||
@@ -17,27 +17,55 @@ class RobotCartesianMotor (PositionerBase):
|
||||
self.setCache(self.doRead(), None)
|
||||
|
||||
def doRead(self):
|
||||
return float("nan") if self.robot.cartesian_destination is None else float(self.robot.cartesian_destination[self.index])
|
||||
return float("nan") if self.robot.cartesian_destination is None else float(self.robot.cartesian_destination[self.name])
|
||||
|
||||
def doWrite(self, value):
|
||||
if self.robot.cartesian_destination is not None:
|
||||
#print "Move " + ROBOT_MOTORS[self.index] + " to " + str(value)
|
||||
self.robot.cartesian_destination[self.index] = float(value)
|
||||
self.robot.set_pnt(robot.cartesian_destination , "tcp_p")
|
||||
self.robot.movel("tcp_p", self.robot.tool , DESC_SCAN)
|
||||
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.index])
|
||||
return float("nan") if self.robot.cartesian_pos is None else float(self.robot.cartesian_pos[self.name])
|
||||
|
||||
def stop(self):
|
||||
robot.stop()
|
||||
robot.reset_motion()
|
||||
robot.resume()
|
||||
|
||||
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
|
||||
self.robot = robot
|
||||
|
||||
def doInitialize(self):
|
||||
self.setCache(self.doRead(), None)
|
||||
|
||||
def doRead(self):
|
||||
return float("nan") if self.robot.spherical_pos is None else robot.spherical_pos[self.name]
|
||||
|
||||
def doWrite(self, value):
|
||||
if self.robot.spherical_destination is not None:
|
||||
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]
|
||||
|
||||
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.name + "_" + ROBOT_JOINT_MOTORS[index], PositionerConfig())
|
||||
PositionerBase.__init__(self, ROBOT_JOINT_MOTORS[index], PositionerConfig())
|
||||
self.index = index
|
||||
self.robot = robot
|
||||
|
||||
@@ -54,9 +82,9 @@ class RobotJointMotor (PositionerBase):
|
||||
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_SCAN)
|
||||
self.robot.movej("tcp_j", self.robot.tool , DESC_DEFAULT)
|
||||
|
||||
def doReadReadback(self):
|
||||
return self.robot.herej()[self.index] if self.robot.joint_pos is None else float(self.robot.joint_pos[self.index])
|
||||
return self.robot.herej()[self.index] if self.robot.joint_pos is None else float(self.robot.joint_pos[self.name])
|
||||
|
||||
|
||||
+124
-47
@@ -7,6 +7,8 @@ run("devices/RobotMotors")
|
||||
class RobotTCP(TcpDevice, Stoppable):
|
||||
def __init__(self, name, server, timeout = 1000, retries = 1):
|
||||
TcpDevice.__init__(self, name, server)
|
||||
self.on_poll_status = {}
|
||||
self.on_poll_status_set = {}
|
||||
self.timeout = timeout
|
||||
self.retries = retries
|
||||
self.header = None
|
||||
@@ -29,11 +31,15 @@ class RobotTCP(TcpDevice, Stoppable):
|
||||
self.known_points = []
|
||||
self.current_points = []
|
||||
self.cartesian_destination = None
|
||||
self.spherical_destination = None
|
||||
#self.flange_pos = [None] * 6
|
||||
self.cartesian_pos = [None] * 6
|
||||
self.joint_pos = [None] * 6
|
||||
self.cartesian_pos = {}
|
||||
self.spherical_pos = {}
|
||||
self.joint_pos = {}
|
||||
self.cartesian_motors_enabled = False
|
||||
self.cartesian_motors = []
|
||||
self.spherical_motors_enabled = False
|
||||
self.spherical_motors = []
|
||||
self.joint_motors_enabled = False
|
||||
self.joint_motors = []
|
||||
self.tool = None
|
||||
@@ -47,7 +53,6 @@ class RobotTCP(TcpDevice, Stoppable):
|
||||
self.default_speed = 100
|
||||
self.latency = 0
|
||||
self.last_msg_timestamp = 0
|
||||
|
||||
self.task_start_retries = 3
|
||||
self.exception_on_task_start_failure = True #Tasks may start and be finished when checked
|
||||
self.simulated_point = ""
|
||||
@@ -68,13 +73,14 @@ class RobotTCP(TcpDevice, Stoppable):
|
||||
def get_tool(self):
|
||||
return self.tool
|
||||
|
||||
def set_frame(self, frame):
|
||||
self.frame = frame
|
||||
self.evaluate("tcp_curframe=" + frame)
|
||||
if self.cartesian_motors_enabled:
|
||||
self.update()
|
||||
self.set_motors_enabled(True)
|
||||
self.waitCacheChange(5000)
|
||||
def set_frame(self, frame, name="tcp_curframe", change_default=True):
|
||||
self.evaluate(name + "=" + frame)
|
||||
if change_default:
|
||||
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
|
||||
@@ -234,7 +240,7 @@ class RobotTCP(TcpDevice, Stoppable):
|
||||
ret = []
|
||||
for i in range(6): ret.append(float(a[i]))
|
||||
return ret
|
||||
|
||||
|
||||
def set_jnt(self, l, name="tcp_j"):
|
||||
self.evaluate(name + "={" + str(l[0]) + ","+ str(l[1]) + ","+ str(l[2]) + ","+ str(l[3]) + ","+ str(l[4]) + ","+ str(l[5]) + "}")
|
||||
|
||||
@@ -631,7 +637,7 @@ class RobotTCP(TcpDevice, Stoppable):
|
||||
elif not self.empty: self.setState(State.Paused)
|
||||
else: self.setState(State.Ready)
|
||||
|
||||
def doUpdate(self):
|
||||
def doUpdate(self):
|
||||
try:
|
||||
start = time.time()
|
||||
cur_task = self.current_task #Can change in background so cache it
|
||||
@@ -670,10 +676,11 @@ class RobotTCP(TcpDevice, Stoppable):
|
||||
|
||||
|
||||
for i in range(6):
|
||||
self.joint_pos[i] = float(sts[8 + i])
|
||||
self.joint_pos[ROBOT_JOINT_MOTORS[i]] = float(sts[8 + i])
|
||||
for i in range(6):
|
||||
self.cartesian_pos[i] = float(sts[14 + i])
|
||||
|
||||
self.cartesian_pos[ROBOT_MOTORS[i]] = float(sts[14 + i])
|
||||
self.update_spherical_pos()
|
||||
|
||||
ev_index = 20 #7
|
||||
ev = sts[ev_index] if len(sts)>ev_index else ""
|
||||
if len(ev.strip()) >0:
|
||||
@@ -692,19 +699,52 @@ class RobotTCP(TcpDevice, Stoppable):
|
||||
"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)
|
||||
self.on_poll_status = {
|
||||
"connected": self.connected,
|
||||
"powered": self.powered,
|
||||
"speed": self.speed,
|
||||
"settled": self.settled,
|
||||
"task": self.current_task,
|
||||
"mode": self.working_mode,
|
||||
"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:
|
||||
m.readback.update()
|
||||
|
||||
if self.spherical_motors_enabled:
|
||||
for m in self.spherical_motors:
|
||||
m.readback.update()
|
||||
if self.joint_motors_enabled:
|
||||
for m in self.joint_motors:
|
||||
m.readback.update()
|
||||
|
||||
|
||||
|
||||
except:
|
||||
if self.state != State.Offline:
|
||||
print >> sys.stderr, "Update error: " + str(sys.exc_info()[1])
|
||||
self.setState(State.Offline)
|
||||
self.setState(State.Offline)
|
||||
|
||||
def on_poll_info(self):
|
||||
on_poll_config_methods ={
|
||||
"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",},
|
||||
"tool": {"cmd":"robot.set_tool", "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}
|
||||
|
||||
#Cartesian space
|
||||
"""
|
||||
@@ -718,7 +758,7 @@ class RobotTCP(TcpDevice, Stoppable):
|
||||
|
||||
#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):
|
||||
def get_cartesian_pos(self, tool=None, frame=None, return_dict=True):
|
||||
if tool is None:
|
||||
self.assert_tool()
|
||||
tool = self.tool
|
||||
@@ -728,24 +768,39 @@ class RobotTCP(TcpDevice, Stoppable):
|
||||
#self.evaluate("tcp_j=herej(); tcp_p=jointToPoint(" + tool + ", " + frame + ", tcp_j)")
|
||||
self.evaluate("tcp_j=herej()")
|
||||
self.evaluate("tcp_p=jointToPoint(" + tool + ", " + frame + ", tcp_j)")
|
||||
return self.get_pnt()
|
||||
x,y,z,rx,ry,rz = self.get_pnt()
|
||||
if return_dict:
|
||||
ret = {"x": x, "y": y, "z": z, "rx": rx, "ry": ry, "rz": rz}
|
||||
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_flange_pos(self, frame=None):
|
||||
return self.get_cartesian_pos(FLANGE, frame)
|
||||
def get_flange_pos(self, frame=None, return_dict=True):
|
||||
return self.get_cartesian_pos(FLANGE, frame, return_dict=return_dict)
|
||||
|
||||
|
||||
def get_cartesian_destination(self, tool=None, frame=None):
|
||||
def get_cartesian_destination(self, tool=None, frame=None,return_dict=True):
|
||||
if tool is None:
|
||||
self.assert_tool()
|
||||
tool = self.tool
|
||||
if frame is None:
|
||||
frame = self.frame
|
||||
return self.here(tool, 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
|
||||
if frame is None:
|
||||
frame = self.frame
|
||||
#return self.here(tool, frame)
|
||||
return self.get_spherical_pos()
|
||||
|
||||
def get_distance_to_pnt(self, name):
|
||||
if self.isSimulated():
|
||||
@@ -754,7 +809,7 @@ class RobotTCP(TcpDevice, Stoppable):
|
||||
else:
|
||||
return 10000
|
||||
#self.here(self.tool, self.frame) #???
|
||||
self.set_pnt(self.get_cartesian_pos() )
|
||||
self.set_pnt(self.get_cartesian_pos(return_dict=False) )
|
||||
return self.distance_p("tcp_p", name)
|
||||
|
||||
def is_in_point(self, p, tolerance = None): #Tolerance in mm
|
||||
@@ -806,28 +861,50 @@ class RobotTCP(TcpDevice, Stoppable):
|
||||
|
||||
def is_emulation(self):
|
||||
return "localhost" in robot.client.getServerAddress()
|
||||
#Cartesian peudo-motors
|
||||
def set_motors_enabled(self, value):
|
||||
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))
|
||||
add_device(self.cartesian_motors[i], True)
|
||||
self.cartesian_destination = self.get_cartesian_destination()
|
||||
else:
|
||||
for m in self.cartesian_motors:
|
||||
remove_device(m)
|
||||
self.cartesian_motors = []
|
||||
self.cartesian_destination = None
|
||||
else:
|
||||
if value:
|
||||
self.cartesian_destination = self.get_cartesian_destination()
|
||||
for m in self.cartesian_motors:
|
||||
m.initialize()
|
||||
|
||||
|
||||
#Cartesian peudo-motors
|
||||
def set_motors_enabled(self, value, pseudos=["cartesian", "spherical"]):
|
||||
if "cartesian" in pseudos:
|
||||
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))
|
||||
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:
|
||||
remove_device(m)
|
||||
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:
|
||||
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)
|
||||
self.spherical_destination = self.get_spherical_pos()
|
||||
self.spherical_pos = self.spherical_destination
|
||||
else:
|
||||
for m in self.spherical_motors:
|
||||
remove_device(m)
|
||||
self.spherical_motors = []
|
||||
self.spherical_destination = None
|
||||
else:
|
||||
if value:
|
||||
self.spherical_destination = self.get_spherical_pos()
|
||||
for m in self.spherical_motors:
|
||||
m.initialize()
|
||||
|
||||
#Joint motors
|
||||
def set_joint_motors_enabled(self, value):
|
||||
if value !=self.joint_motors_enabled:
|
||||
self.joint_motors_enabled = value
|
||||
@@ -849,7 +926,7 @@ class RobotTCP(TcpDevice, Stoppable):
|
||||
self.assert_tool()
|
||||
if desc is None: desc = self.default_desc
|
||||
if point is None:
|
||||
self.set_pnt(self.get_cartesian_pos() )
|
||||
self.set_pnt(self.get_cartesian_pos(return_dict=False) )
|
||||
else:
|
||||
self.set_pnt(point)
|
||||
np=self.eval_trf('align(tcp_p.trsf, ' + frame + '.trsf)')
|
||||
|
||||
Reference in New Issue
Block a user