352 lines
14 KiB
Python
352 lines
14 KiB
Python
import math
|
|
KNOWN_POINTS = P_PARK, P_HOME = "park", "home"
|
|
TASKS = MOVE_PARK, MOVE_HOME, TWEAK_X , TWEAK_Y = "movePark", "moveHome", "tweakX", "tweakY"
|
|
|
|
DESCS = DESC_FAST,DESC_SLOW, = "mNomSpeed", "mNomSpeed"
|
|
DESC_DEFAULT = DESCS[0]
|
|
|
|
FLANGE = "flange"
|
|
|
|
TOOLS = ["t_Detector",]
|
|
TOOL_DEFAULT = TOOL_DET = TOOLS[0]
|
|
|
|
FRAMES = ["f_actualFrame", "f_Experiment1", "f_Experiment2", "f_Experiment3"]
|
|
FRAME_DEFAULT = FRAMES[1]
|
|
|
|
DEFAULT_ROBOT_POLLING = 200
|
|
TASK_WAIT_ROBOT_POLLING = 50
|
|
DEFAULT_SPEED=100
|
|
|
|
run("devices/RobotTCP")
|
|
|
|
|
|
simulation = True
|
|
|
|
|
|
class RobotBernina(RobotTCP):
|
|
def __init__(self, name, server, timeout = 1000, retries = 1):
|
|
RobotTCP.__init__(self, name, server, timeout, retries)
|
|
self.set_tasks(TASKS)
|
|
self.set_known_points(KNOWN_POINTS)
|
|
self.setPolling(DEFAULT_ROBOT_POLLING)
|
|
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 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_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])
|
|
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, 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:
|
|
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):
|
|
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, 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:
|
|
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]
|
|
|
|
# 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):
|
|
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()
|
|
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:
|
|
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:
|
|
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):
|
|
self.start_task(MOVE_HOME)
|
|
self.wait_task_finished(TASK_WAIT_ROBOT_POLLING)
|
|
self.assert_home()
|
|
|
|
def move_park(self):
|
|
if not self.is_in_point(P_PARK):
|
|
self.start_task(MOVE_PARK)
|
|
self.wait_task_finished(TASK_WAIT_ROBOT_POLLING)
|
|
self.assert_park()
|
|
|
|
def tweak_x(self, offset):
|
|
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)
|
|
self.wait_task_finished(TASK_WAIT_ROBOT_POLLING)
|
|
|
|
def on_event(self,ev):
|
|
#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:
|
|
if task==MOVE_HOME: self.simulated_point=P_HOME
|
|
elif task==MOVE_PARK: self.simulated_point=P_PARK
|
|
else: self.simulated_point=None
|
|
|
|
def doUpdate(self):
|
|
#start = time.time()
|
|
RobotTCP.doUpdate(self)
|
|
|
|
def start_task(self, program, *args, **kwargs):
|
|
#TODO: Check safe position
|
|
self.task_name = program
|
|
self.task_args = args
|
|
self.task_kwargs = kwargs
|
|
ret = RobotTCP.start_task(self, program, *args, **kwargs)
|
|
|
|
|
|
def stop_task(self):
|
|
RobotTCP.stop_task(self)
|
|
#TODO: Restore safe position
|
|
|
|
|
|
def set_remote_mode(self):
|
|
self.set_profile("remote")
|
|
|
|
def set_local(self):
|
|
self.set_profile("default")
|
|
|
|
def is_park(self):
|
|
return self.is_in_point(P_PARK)
|
|
|
|
def is_home(self):
|
|
return self.is_in_point(P_HOME)
|
|
|
|
def is_cleared(self):
|
|
return self.get_current_point() is not None
|
|
|
|
def assert_park(self):
|
|
self.assert_in_point(P_PARK)
|
|
|
|
def assert_home(self):
|
|
self.assert_in_point(P_HOME)
|
|
|
|
def assert_cleared(self):
|
|
if not self.is_cleared():
|
|
raise Exception("Robot not in cleared position")
|
|
|
|
def wait_ready(self):
|
|
self.waitState(State.Ready, 1000) #robot.state.assertReady()
|
|
|
|
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})
|
|
|
|
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)
|
|
|
|
#robot.latency = 0.005
|
|
robot.set_default_desc(DESC_DEFAULT)
|
|
robot.default_speed = DEFAULT_SPEED
|
|
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)
|