def try_connect():
global robot
robot = Dorna()
result = json.loads(robot.connect(port_name='/dev/cu.usbmodem1424401'))
if (result["connection"] == 2):
print("Connected successfully to robot at port \"" + result["port"] +
"\"")
return True
else:
return False
class SpectroAlign(object):
rest_cmd = {
"command": "move",
"prm": {
"movement": 0,
"path": "joint",
"speed": 1000,
"j0": 0.,
"j1": 145.,
"j2": -90,
"j3": 0.0,
"j4": 0.0
}
}
angle_cmd = {
"command": "move",
"prm": {
"movement": 0,
"path": "joint",
"a": -90,
"b": 0
}
}
start_cmd = {
"command": "move",
"prm": {
"movement": 0,
"path": "joint",
"speed": 1000,
"j0": -7,
"j1": 50.,
"j2": -50.
}
}
vertical_cmd = {
"command": "move",
"prm": {
"movement": 0,
"path": "joint",
"speed": 500,
"j0": 0.,
"j1": 90.,
"j2": 0.,
"j3": 0.0,
"j4": 0.0
}
}
reset_j0 = {
"command": "move",
"prm": {
"movement": 0,
"path": "joint",
"speed": 1000,
"j0": 0.
}
}
def __init__(self,
robot=None,
center=None,
rel_pos=[0., 0., 0.],
j0_offset=0.,
data_dir=None):
"""
This creates the python object, we can specify initial conditions for the robot
Typically, we assume we're starting from the rest position, pointed away from the table.
"""
if center == None:
self.center = [0., 0., 0., 0., 0.]
self.center_set = False
else:
self.center = center
self.center_set = True
if data_dir == None:
self.data_dir = '/home/fireball2/SpectroAlign/' + datetime.now(
).strftime("%y%m%d") + '/'
else:
self.data_dir = data_dir
self.log_str = self.data_dir + "logfile"
#self.logfile = open(self.data_dir + "logfile", 'a')
self.rel_pos = rel_pos
self.j0_offset = j0_offset
if robot == None:
self.robot = Dorna()
else:
self.robot = robot
def __repr__(self):
return self.robot.device()
def connect(self, port="/dev/ttyACM0"):
"""
connects the robot arm.
input:
port: (string) port address for robot
"""
self.robot.connect(port)
return self.robot.device()
def checkout(self):
"""
Disconnect robot and terminate robot dorna object
"""
self.robot.disconnect()
self.robot.terminate()
return self.robot.device()
def calibrate(self, position=[0., 90., 0., 0., 0.]):
"""
Defines current joint position of the robot in the Dorna joint system.
defaults to calibrating on the stretched upwards position
input:
position: (list of floats) [j0,j1,j2,j3,j4] in degrees
returns:
new joint values
"""
self.completion()
self.robot.calibrate(position)
self.robot.save_config()
return self.robot.position()
def log_line(self, line):
"""
Write line to logfile
"""
logfile = open(self.log_str, 'a')
logfile.write(line)
logfile.close()
return None
#### Robot motion and dorna parameter methods
def play(self, cmd):
"""
passes Dorna command to embeded Dorna object
input:
cmd: (dict) in Dorna format for motions
output:
dorna play command output
"""
output = self.robot.play(cmd)
self.find_rel()
return output
def home(self, joint):
"""
passes joint name to be homed
input:
joint: (string) name of joint
output:
dorna play command output
"""
output = self.robot.home(joint)
return output
def set_center(self, coords=None):
"""
Sets center of field coordinates at current robot position, or at specified coords.
input:
coords: (list of floats) x,y,z,a,b position of arm in dorna coordinates
output:
(list of floats) position of new center in dorna xyzab format
"""
if coords == None:
self.center = json.loads(self.robot.position("xyz"))
else:
self.center = coords
print("Fiber Center :" + str(self.center))
self.log_line("\nSet Fiber Center [mm]: " + str(self.center))
self.center_set = True
return self.center
def find_rel(self):
"""
Calculates and updates relative position to center of field
"""
robot_pos = json.loads(self.robot.position("xyz"))
x = robot_pos[0] - self.center[0]
y = robot_pos[1] - self.center[1]
z = robot_pos[2] - self.center[2]
self.rel_pos = [x, y, z]
print("relative position : " + str(self.rel_pos))
return self.rel_pos
def to_center(self):
"""
Returns the robot to the center of field as recorded.
"""
if self.center_set:
center_cmd = {
"command": "move",
"prm": {
"movement": 0,
"path": "line",
"xyz": self.center
}
}
self.play(center_cmd)
else:
print("Center coordinate has not been set")
return None
def to_xyz(self, coords):
"""
Moves robot to given position in xyz
"""
motion_cmd = {
"command": "move",
"prm": {
"movement": 0,
"path": "line",
"xyz": coords
}
}
self.play(motion_cmd)
self.completion()
self.find_rel()
return None
def to_joint(self, coords):
"""
Moves robot to given position in joint coord.
"""
motion_cmd = {
"command": "move",
"prm": {
"movement": 0,
"path": "line",
"joint": coords
}
}
self.play(motion_cmd)
self.completion()
self.find_rel()
return None
def ax_rotation(self, offset):
"""rotates around z and redefines x, y coordinates of the robot.
j0 offset: degrees in counterclockwise direction looking from above.
"""
self.play(SpectroAlign.rest_cmd)
self.play({
"command": "move",
"prm": {
"movement": 0,
"speed": 1000,
"path": "joint",
"j0": offset
}
})
self.j0_offset += offset
self.completion()
self.robot.set_joint({"j0": 0})
return None
def completion(self):
"""
Wait until previous robot motion is completed.
"""
status = self.robot.device()
parsed_status = json.loads(status)
while parsed_status['state'] != 0:
time.sleep(0.5)
status = self.robot.device()
parsed_status = json.loads(status)
#print("Command is done. On to the next!")
return None
def xyz_motion(self, coord, value, speed=2500):
"""
Moves the fiber in x y z directions
input:
speed: (float) unit length/minute
coord: (string) "x", "y" or "z" for direction of motion
value: (float)
"""
self.completion()
if coord == "x":
grid_x = {
"command": "move",
"prm": {
"movement": 1,
"speed": speed,
"path": "line",
"x": value
}
}
self.play(grid_x)
elif coord == "z":
grid_z = {
"command": "move",
"prm": {
"movement": 1,
"speed": speed,
"path": "line",
"z": value
}
}
self.play(grid_z)
elif coord == "y":
grid_y = {
"command": "move",
"prm": {
"movement": 1,
"speed": speed,
"path": "line",
"y": value
}
}
self.play(grid_y)
else:
print("Invalid coordinate command")
self.completion()
self.find_rel()
return None
def joint_motion(self, coord, value, speed=1000, movement=1):
"""
Shorthand function to move robot joints
input:
robot: (Dorna object)
coord: (string) "j0", "j1", "j2", "j3" or "j4" for direction of motion
value: (float) angle in degrees
speed: (float) degrees/min
movement: 1 for relative, 0 for absolute
"""
self.completion()
if coord == "j0":
cmd = {
"command": "move",
"prm": {
"movement": movement,
"speed": speed,
"path": "joint",
"j0": value
}
}
self.play(cmd)
elif coord == "j1":
cmd = {
"command": "move",
"prm": {
"movement": movement,
"speed": speed,
"path": "joint",
"j1": value
}
}
self.play(cmd)
elif coord == "j2":
cmd = {
"command": "move",
"prm": {
"movement": movement,
"speed": speed,
"path": "joint",
"j2": value
}
}
self.play(cmd)
elif coord == "j3":
cmd = {
"command": "move",
"prm": {
"movement": movement,
"speed": speed,
"path": "joint",
"j3": value
}
}
self.play(cmd)
elif coord == "j4":
cmd = {
"command": "move",
"prm": {
"movement": movement,
"speed": speed,
"path": "joint",
"j4": value
}
}
self.play(cmd)
else:
print("Invalid coordinate command")
self.completion()
self.find_rel()
return None
def find_pos(self):
"""
returns position in dorna xyz coordinates as list
output:
(list of floats) xyzab coordinates
"""
position = json.loads(self.robot.position("xyz"))
print("x,y,z,a,b =" + str(position))
return position
### Nuvu control methods
def set_log(self):
"""
Creates log file for image taking, sets up camserver image path
output
(textio) pointer to file.
"""
#data_dir = '/home/fireball2/SpectroAlign/' + datetime.now().strftime("%y%m%d") +'/'
command = "mkdir -p " + self.data_dir
p = Popen(command,
shell=True,
stdin=PIPE,
stdout=PIPE,
stderr=STDOUT,
close_fds=True)
#logfile = open(data_dir+'logfile', 'a')
command = 'cam path ' + self.data_dir
p = Popen(command,
shell=True,
stdin=PIPE,
stdout=PIPE,
stderr=STDOUT,
close_fds=True)
output = p.stdout.read()
self.log_line('Data directory=' + self.data_dir + "\n")
return None
def expose(self, exptime, burst=1):
# Take images
command = 'cam imno'
p = Popen(command,
shell=True,
stdin=PIPE,
stdout=PIPE,
stderr=STDOUT,
close_fds=True)
imno1 = p.stdout.read()
#self.logfile.write('\nimage%06d.fits' % int(imno1))
command0 = 'cam burst=' + str(burst)
print(command0)
# Run command
p = Popen(command0,
shell=True,
stdin=PIPE,
stdout=PIPE,
stderr=STDOUT,
close_fds=True)
# Write to the log file
bpar = p.stdout.read()
#self.logfile.write('\nBurst param= %s' % str(bpar))
#subprocess.call(command0,shell=True)
# Set the exposure time
command1 = 'cam exptime=' + str(exptime)
print(command1)
# Run command
p = Popen(command1,
shell=True,
stdin=PIPE,
stdout=PIPE,
stderr=STDOUT,
close_fds=True)
expout = p.stdout.read()
#self.logfile.write('\nExposure time= %s' % str(expout))
#subprocess.call(command1,shell=True)
# command2 = './cam emgain='+str(emgain)
# print command2
# Run command
# p = Popen(command2, shell=True, stdin=PIPE, stdout=PIPE, stderr=STDOUT, close_fds=True)
# Write to the log file
# output = p.stdout.read()
# logfile.write('EM gain= %s' % str(output))
#subprocess.call(command2,shell=True)
command3 = 'cam expose'
print(command3)
# Run command
p = Popen(command3,
shell=True,
stdin=PIPE,
stdout=PIPE,
stderr=STDOUT,
close_fds=True)
# Write to the log file
for i in range(burst):
self.log_line('\nimage%06d.fits' % str(imno1 + i))
self.log_line('\t%s' % str(expout))
self.log_line('\t%s' % str(bpar))
self.log_line('\t%s' % str(burst))
self.log_line('\t%s' % str(self.rel_pos[0]))
self.log_line('\t%s' % str(self.rel_pos[1]))
self.log_line('\t%s' % str(self.rel_pos[2]))
time.sleep(exptime * burst)
return None
### Combined routines (exposures + motion)
def focus_test(self,
steps=10,
dz=0.5,
exptime=1.,
burst=1,
dx0=0.,
dy0=0.,
dz0=0.):
"""
Takes through focus image bursts as the robot moves upwards in z.
input:
steps: (int) number changes in position
dz: (float) separation between steps in mm
exptime = (float) exposure in seconds
burst = (int) number of images per exposure
dx0 = (float) displacement from original x position in mm
dy0 = (float) displacement from original y position in mm
dz0 = (float) displacement from original z position in mm
"""
print("Sweeping focus on Z")
start_coord = json.loads(self.robot.position("xyz"))
return_cmd = {
"command": "move",
"prm": {
"movement": 0,
"path": "line",
"xyz": start_coord
}
}
if dx0 != 0.:
self.xyz_motion("x", dx0)
if dy0 != 0.:
self.xyz_motion("y", dy0)
if dz0 != 0.:
self.xyz_motion("z", dz0)
for i in range(steps):
z_cmd = {
"command": "move",
"prm": {
"movement": 0,
"path": "line",
"z": start_coord[2] + (dz * i)
}
}
self.play(z_cmd)
self.completion()
print("z = " + str(start_coord[2] + dz))
print('\nRelative position [mm] %s' % str(self.rel_pos))
self.expose(exptime, burst)
print("Sweep complete, returning to start position")
self.play(return_cmd)
self.completion()
return None
class MoveDorna:
def __init__(self):
path = os.path.dirname(os.path.abspath(sys.argv[0]))
path = path + "/config/dorna.yaml"
# Objekt der Dorna-Klasse instanziieren
self.robot = Dorna(path)
# Verbindung mit dem Roboter herstellen
self.robot.connect()
# Homing-Prozess
def home(self):
proceed = False
while not proceed:
_input = input("Home Dorna? (y/n)")
if _input == "y":
proceed = True
elif _input == "n":
self.terminate()
break
if proceed:
# Dorna in die Home-Position fahren
self.robot.home(["j0", "j1", "j2", "j3"])
print(self.robot.homed())
print(self.robot.position())
proceed = False
while not proceed:
_input = input("j3 und j4 auf 0 setzen? (y/n)")
if _input == "y":
proceed = True
elif _input == "n":
break
if proceed:
# Gelenke des Endeffektors nullen
self.set_joint_j3_0()
def set_joint_j3_0(self):
self.robot.set_joint({"j3": 0})
self.robot.set_joint({"j4": 0})
# Endeffektor zu einer bestimmten Koordinate fahren
def move_to_position(self):
# Koordinate eingeben
i = Input()
output = i.input_values()
x = output.get("x")
y = output.get("y")
z = output.get("z")
# Koordinate berechnen
position = JointValue(x, y, z)
position_result = position.calc_joint_values()
position_array = self.get_joints(position_result)
print(position_array)
proceed = True
while proceed:
_input = input("Bewegung ausführen? (y/n)")
if _input == "n":
proceed = False
elif _input == "y":
break
if proceed:
# Bewegung ausführen
self.move_dorna(position_array)
# Ablaufsteuerung zum Bälle einsammeln ausführen
def collect_balls(self):
n = eval(input("Anzahl der Bälle: "))
balls_array = []
out_of_range = False
# Koordinaten der Bälle eingeben
for j in range(n):
print("Position des", j + 1, ". Balls:")
i = Input()
output = i.input_values()
x = output.get("x")
y = output.get("y")
z = output.get("z")
# Werte der Gelenke berechnen
seven_above_ball = JointValue(x, y, z + 7) # 7cm über dem Ball
five_above_ball = JointValue(x, y, z + 5) # 5cm über dem Ball
one_above_ball = JointValue(x, y, z + 1) # 1cm über dem Ball
seven_above_ball_result = seven_above_ball.calc_joint_values()
five_above_ball_result = five_above_ball.calc_joint_values()
one_above_ball_result = one_above_ball.calc_joint_values()
if five_above_ball == 1 or one_above_ball == 1:
out_of_range = True
break
seven_above_ball_array = self.get_joints(seven_above_ball_result)
five_above_ball_array = self.get_joints(five_above_ball_result)
one_above_ball_array = self.get_joints(one_above_ball_result)
balls_dict = {"1.": five_above_ball_array, "2.": one_above_ball_array, "3.": seven_above_ball_array}
balls_array.append(balls_dict)
print("Position des Behälters: ")
# Koordinate des Behälters angeben
i = Input()
output = i.input_values()
x = output.get("x")
y = output.get("y")
z = output.get("z")
# Werte der Gelenke berechnen
fifteen_above_container = JointValue(x, y, z + 15) # 15cm über dem Behälter
fifteen_above_container_result = fifteen_above_container.calc_joint_values()
if five_above_ball == 1 or one_above_ball == 1:
out_of_range = True
fifteen_above_container_array = self.get_joints(fifteen_above_container_result)
print(balls_array)
balls_array_len = len(balls_array)
if not out_of_range:
for x in range(balls_array_len):
print(balls_array[x])
print(fifteen_above_container_array)
proceed = True
while proceed:
_input = input("Bewegung ausführen? (y/n)")
if _input == "n":
proceed = False
elif _input == "y":
break
if proceed:
for i in range(n):
first = balls_array[i].get("1.")
second = balls_array[i].get("2.")
third = balls_array[i].get("3.")
# Ablaufsteuerung
self.open_gripper()
self.move_dorna(first)
self.move_dorna(second)
self.close_gripper()
self.move_dorna(third)
self.move_dorna(fifteen_above_container_array)
self.open_gripper()
print("Bewegung ausgeführt.")
else:
print("Bewegung konnte nicht ausgeführt werden")
def terminate(self):
self.robot.terminate()
def close_gripper(self):
self.robot.set_io({"servo": 700})
def open_gripper(self):
self.robot.set_io({"servo": 0})
def get_joints(self, height):
j0 = height.get("j0")
j1 = height.get("j1")
j2 = height.get("j2")
j3 = height.get("j3")
return [j0, j1, j2, j3, 0]
def move_dorna(self, position):
self.robot.move({"movement": 0, "path": "joint", "joint": position})
# Nullposition
def zero(self):
self.robot.move({"movement": 0, "path": "joint", "joint": [0, 0, 0, 0, 0]})
from dorna import Dorna
import json
import time
# creat Dorna object and connect to the robot
robot = Dorna()
robot.connect()
# home all the joints
robot.home(["j0", "j1", "j2", "j3"])
"""
move to j0 = 0, ..., j4 = 0
wait for the motion to be done, timeout = 1000 seconds
"""
result = robot.play({
"command": "move",
"prm": {
"path": "joint",
"movement": 0,
"joint": [0, 0, 0, 0, 0]
}
})
result = json.loads(result)
wait = robot._wait_for_command(result, time.time() + 1000)
# move in cartesian space, -10 inches toward X direction
robot.play({
"command": "move",
"prm": {
"path": "line",
"movement": 1,
class Arm(object):
def __init__(self):
self.robot = Dorna("./config.yaml")
def go(self, position=HOME):
print("Go to ", position)
tmpcmd = {"command": "move", "prm": position}
self.robot.play(tmpcmd)
"""============================================================
CONNECTION AND HOMING THE ROBOT
===========================================================
"""
def connect(self):
return self.robot.connect()
def dis(self):
return self.robot.disconnect()
def connectAndHomeIfneed(self):
self.robot.connect()
self.homeIfneed()
def connectAndHomes(self):
self.robot.connect()
self.robot.home("j0")
self.robot.home("j1")
self.robot.home("j2")
self.robot.home("j3")
def isConnected(self):
device = json.loads(r.robot.device())
if device['connection'] == 2:
return True
else:
return False
def isHomed(self):
if not self.isConnected():
print("Robot is not connected")
return False
homed = c
for key in homed:
if homed[key] == 1:
print(key, " is homed")
else:
print(key, " is not homed yet")
def homes(self):
self.robot.home("j0")
self.robot.home("j1")
self.robot.home("j2")
self.robot.home("j3")
def homeIfneed(self):
if not self.isConnected():
print("Robot is not connected")
return False
homed = json.loads(self.robot.homed())
for key in homed:
if homed[key] == 1:
print(key, " is homed")
else:
print("Homing ", key)
self.robot.home(key)
"""===================================================================
GET CURRENT POSITION AS XYZ SYSTEM
==================================================================
"""
def posXYZ(self):
return json.loads(self.robot.position('xyz'))
"""===================================================================
GET CURRENT POSITION AS JOINTS SYSTEM
==================================================================
"""
def pos(self):
return json.loads(self.robot.position())
"""==============================================
ADJUST the joints
=================================================
"""
#pass in joint and value as parameter ex:adjust("j0",10)
def adjust(self, joint, degrees: float, relatively=True):
# creates a position-movement description that can be fed to go().
tempPath = deepcopy(PRM_TEMPLATE)
for j in JOINTS:
if j == joint:
tempPath[j] = degrees
break
tempPath["movement"] = 1 if relatively else 0
self.go(tempPath)
return tempPath
#pass in joints object as parameter ex:adjust({"j1":0,"j2":0})
def adjustJoints(self, joints, relatively=True):
# creates a position-movement description that can be fed to go().
tempPath = deepcopy(PRM_TEMPLATE)
print(joints)
for key in joints:
for validJoint in JOINTS:
if key == validJoint:
tempPath[validJoint] = joints[key]
tempPath["movement"] = 1 if relatively else 0
self.go(tempPath)
return tempPath
"""==================================================
MOVE with x,y,z Coordinate System
=================================================
"""
#pass in axis and value as parameter
#ex:moveToPosition('z',10)
def adjustAxis(self, axis, degrees, relatively=True):
tempPath = deepcopy(PRM_TEMPLATE)
tempPath['path'] = 'line'
for c in COORDINATE:
if c == axis:
tempPath[c] = degrees
break
tempPath["movement"] = 1 if relatively else 0
self.go(tempPath)
return tempPath
"""==================================================
MOVE with xyz Coordinate System
=================================================
"""
#pass in coordinate object as parameter
#ex:moveToCoordinate({'x':10,'y':20,'z':10})
def adjustCoordinate(self, coor, relatively=True):
tempPath = deepcopy(PRM_TEMPLATE)
tempPath['path'] = 'line'
for key in coor:
for validJoint in COORDINATE:
if key == validJoint:
tempPath[validJoint] = coor[key]
tempPath["movement"] = 1 if relatively else 0
self.go(tempPath)
return tempPath
"""==================================================
wait
=================================================
"""
def wait(self, value):
time.sleep(value)
def goHome(self):
self.go(HOME)
def goFlat(self):
self.go(FLAT)
"""==================================================
MOVE tools head relatively to current angle
=================================================
"""
def adjustHead(self, angle):
self.adjust('j3', angle)
return self.pos()[3]
"""==================================================
MOVE tools head to specific Angle
=================================================
"""
def setHeadAngle(self, angle):
self.adjust('j3', angle, False)
return self.pos()[3]
"""==================================================
MOVE with specific directin
=================================================
"""
def move(self, direct: Direction.FORWARD, value: float):
if direct == Direction.FORWARD:
self.adjustAxis('x', value)
if direct == Direction.BACKWARD:
moveAxis('x', value * -1)
if direct == Direction.UP:
self.adjustAxis('z', value)
if direct == Direction.DOWN:
self.adjustAxis('z', value * -1)
if direct == Direction.LEFT:
self.adjustAxis('y', value)
if direct == Direction.RIGHT:
self.adjustAxis('y', value * -1)
"""==================================================
MOVE move to a coordiation
ex:moveTo((1,2,6)) x=1,y=2,z6
=================================================
"""
def moveTo(self, xyz):
self.adjustCoordinate({'x': xyz[0], 'y': xyz[1], 'z': xyz[2]}, False)
"""==================================================
Collection of Motion
"""
def pickIt(self):
self.adjustJoints({'j1': 45, 'j2': -90, 'j3': 45, 'j4': 0}, False)
self.adjustCoordinate({'x': 15, 'y': 0, 'z': 3.5}, False)
self.wait(10)
self.adjustCoordinate({'x': 16, 'y': 0, 'z': 10}, False)
self.adjust('j0', 180)
self.adjustCoordinate({'x': 4, 'y': 0, 'z': -5})
self.adjustCoordinate({'x': 0, 'y': 0, 'z': -3})
self.wait(10)
self.adjustCoordinate({'x': -5, 'y': 0, 'z': 8})
self.adjust('j0', -180)
self.adjustCoordinate({'x': 15, 'y': 0, 'z': 3.5}, False)
self.wait(10)
self.go()
def preHit(self):
self.adjustJoints({
'j0': 0,
'j1': 45,
'j2': -123,
'j3': 78,
'j4': 0
}, False)
self.adjustCoordinate({'x': -10, 'y': 0, 'z': 0})
self.adjustCoordinate({'x': 60, 'y': 0, 'z': 0})
def hit(self):
self.adjustCoordinate({'x': 1, 'y': 0, 'z': 0})
for i in [0, 1, 2, 3, 4]:
self.adjustCoordinate({'x': -60, 'y': 0, 'z': 0})
self.adjustCoordinate({'x': 60, 'y': 0, 'z': 0})
self.wait(10)
self.adjustCoordinate({'x': -10, 'y': 0, 'z': 0})
self.adjustCoordinate({'x': -10, 'y': 0, 'z': 0})
def preHitTop(self):
self.adjustJoints({
'j0': 0,
'j1': 45,
'j2': -123,
'j3': 78,
'j4': 0
}, False)
self.adjustCoordinate({'x': 0, 'y': 0, 'z': 80})
self.adjustCoordinate({'x': 80, 'y': 0, 'z': 0, 'a': -90})
self.adjustCoordinate({'x': 0, 'y': 0, 'z': 14})
def hitTop(self):
self.adjustCoordinate({'x': 0, 'y': 0, 'z': -1})
for i in [0, 1, 2, 3, 4]:
self.adjustCoordinate({'x': 0, 'y': 0, 'z': 20})
self.adjustCoordinate({'x': 0, 'y': 0, 'z': -20})
self.wait(6)
def prRight(self):
self.adjustJoints({
'j0': 0,
'j1': 45,
'j2': -123,
'j3': 78,
'j4': 0
}, False)
def hitRight(self):
self.adjustCoordinate({'x': 0, 'y': 1, 'z': 0})
for i in [0, 1, 2, 3, 4]:
self.adjustCoordinate({'x': 0, 'y': -20, 'z': 0})
self.adjustCoordinate({'x': 0, 'y': 20, 'z': 0})
self.wait(6)
class Subscriber:
def __init__(self):
rospy.init_node("coordinates_subscriber")
self.coordinates_sub = rospy.Subscriber("/coordinates", Coordinates,
self.callback)
self.coordinates_msg = Coordinates()
path = os.path.dirname(os.path.abspath(sys.argv[0]))
path = path + "/config/dorna.yaml"
self.robot = Dorna(path)
self.robot.connect()
def home(self):
proceed = False
while not proceed:
_input = input("Home Dorna? (y/n)")
if _input == "y":
proceed = True
elif _input == "n":
self.terminate()
break
if proceed:
self.robot.home(["j0", "j1", "j2", "j3"])
proceed = False
while not proceed:
_input = input("j3 auf 0 setzen? (y/n)")
if _input == "y":
proceed = True
elif _input == "n":
break
if proceed:
self.set_joint_j3_0()
def set_joint_j3_0(self):
self.robot.set_joint({"j3": 0})
self.robot.set_joint({"j4": 0})
def callback(self, msg):
self.coordinates_msg = msg
def collect_balls(self):
container_coordinate = self.coordinates_msg.container_coordinates
if len(container_coordinate) > 0:
x_c = container_coordinate[0]
y_c = container_coordinate[1]
z_c = 0
print("Behälterposition: ", x_c, y_c)
ball_coordinate = self.coordinates_msg.ball_coordinates
balls_array = []
if len(ball_coordinate) > 0:
for i in range(0, len(ball_coordinate), 2):
x_b, y_b = ball_coordinate[i], ball_coordinate[i + 1]
z_b = 0
print("Ballposition: ", x_b, y_b)
five_above_ball = JointValue(x_b, y_b, z_b + 5)
one_above_ball = JointValue(x_b, y_b, z_b + 1)
try:
five_above_ball_result = five_above_ball.calc_joint_values(
)
one_above_ball_result = one_above_ball.calc_joint_values()
except ValueError:
print("Ball außerhalb des Bereiches")
continue
five_above_ball_array = self.get_joints(five_above_ball_result)
one_above_ball_array = self.get_joints(one_above_ball_result)
balls_dict = {
"1.": five_above_ball_array,
"2.": one_above_ball_array,
"3.": five_above_ball_array
}
balls_array.append(balls_dict)
ten_above_container = JointValue(x_c, y_c, z_c + 10)
try:
ten_above_container_result = ten_above_container.calc_joint_values(
)
except ValueError:
print("Behälter außerhalb des Bereiches")
ten_above_container_array = self.get_joints(
ten_above_container_result)
for i in balls_array:
first = i.get("1.")
second = i.get("2.")
third = i.get("3.")
self.open_gripper()
self.move_dorna(first)
self.move_dorna(second)
self.close_gripper()
self.move_dorna(third)
self.move_dorna(ten_above_container_array)
self.open_gripper()
else:
print("Konnte keine Bälle orten, versuche erneut")
rospy.sleep(1)
self.collect_balls()
def terminate(self):
self.robot.terminate()
def close_gripper(self):
self.robot.set_io({"servo": 675})
def open_gripper(self):
self.robot.set_io({"servo": 0})
def move_dorna(self, position):
self.robot.move({"movement": 0, "path": "joint", "joint": position})
def get_joints(self, height):
j0 = height.get("j0")
j1 = height.get("j1")
j2 = height.get("j2")
j3 = height.get("j3")
return [j0, j1, j2, j3, 0]
class DornaRobot:
def __init__(self):
self._config_path = os.path.dirname(
os.path.realpath(__file__)) + str("/config.yaml")
self.params = list(yaml.safe_load(open(self._config_path, 'r')).keys())
self.joint_names = ['j0', 'j1', 'j2', 'j3', 'j4']
self.axis_names = ['x', 'y', 'z', 'a', 'b']
self._robot = Dorna(self._config_path)
self._default_home = dict(zip(self.joint_names, [0, 145, -90, 0, 0]))
self.connection_status = None
self._elevator_demo = False
self._hand = True
self.triggered = False
def init(self, home=True):
'''
Connect to the robot and then home or set the stepper motors
'''
try:
self.connect()
except ConnectionException:
logger.error(
"ConnectionError [dorna_robot]: There was a connection issue. Check Robot and try again"
)
raise
else:
if home:
self.home()
else:
self.set_joint()
logger.info("Robot startup success")
################################
#### Command calls to robot ####
################################
def calibrate(self):
'''
Set angle offsets for homeing
'''
joints = self.get_joint_angles()
self._robot.calibrate(joints)
self._robot.save_config(save_path=self._config_path)
self.set_joint()
def connect(self, usb=None):
'''
Connects to Dorna\n
Note: Make sure to setup dorna udev rule
'''
logger.info('Connecting to Dorna...')
if not usb:
usb = "/dev/actuators/dorna"
response = json.loads(self._robot.connect(port_name=usb))
self.connection_status = self.get_connection_status()
while response["connection"] == DornaConnection.CONNECTING:
time.sleep(0.5)
response = json.loads(self._robot.device())
# if response["connection"] == DornaConnection.CONNECTED:
if response["connection"] == DornaConnection.DISCONNECTED:
logger.error("ConnectionError")
logger.info(
"There was a connection issue. Check Robot and try again")
raise ConnectionException()
def disconnect(self):
'''
Disconects from Arduino
'''
self._robot.disconnect()
def halt(self):
'''
Stops Dorna and delets queue
'''
self._robot.halt()
def force_fulfill(self, joint, desired_pose):
'''
Waits until joint angle equal to desired pose\n
Note: only works for single joint and angle
'''
# TODO: Make this better
not_in_position = True
while not_in_position:
joint_angles = self.get_joint_angles()
if int(joint_angles[joint]) == desired_pose:
not_in_position = False
def force_fulfill_joints(self, desired):
not_in_position = True
while not_in_position and not self.triggered:
current = self.get_joint_angles()
check = np.linalg.norm(
np.diff(np.row_stack((current, desired)), axis=0))
not_in_position = check >= 0.1
def force_fulfill_xyzab(self, desired):
not_in_position = True
while not_in_position and not self.triggered:
current = self.get_cartesian_position()
check = np.linalg.norm(
np.diff(np.row_stack((current, desired)), axis=0))
not_in_position = check >= 0.1
def elevator_demo_safe_home(self, i):
'''
Turn the base 90 degrees before calibrating next motor\n
Then move base back to origin
'''
if i == 1:
self.move_joints(path="joint",
movement=DornaMovement.GLOBAL.value,
speed=DornaMovement.DEFAULT_JOINT_SPEED.value,
joint_angles={'j0': -90})
self.force_fulfill(0, -90)
elif i == 2:
self.move_joints(path="joint",
movement=DornaMovement.GLOBAL.value,
speed=DornaMovement.DEFAULT_JOINT_SPEED.value,
joint_angles={'j0': 0})
self.force_fulfill(0, 0)
def home(self):
'''
Calibrate stepper motors
'''
logger.info('Homing Robot...')
try:
## Only homing joints 0-2 and setting joints 3-4 so that tool head does not colide with robot
if self._hand:
n = 3
self.set_joint({"j3": 0, "j4": 0})
else:
n = len(self.joint_names)
for i in range(n):
if self._elevator_demo:
self.elevator_demo_safe_home(i)
motor_name = "j" + str(i)
response = json.loads(self._robot.home(motor_name))
if response[motor_name] != 1:
raise HomingException()
logger.info('Finished Homing Robot...')
except HomingException:
logger.error(
'HomingError [dorna_robot]: Motor {} was unable to home'.
format(i))
def jog(self, path, cmd_dict):
'''
Jogs single motor or robot in single direction\n
path = 'joint' or 'line'\n
cmd_dict = dictionary of joint_name or axis_name as keys and angle or position as values
'''
logger.info('Jogging...')
if path == "joint":
speed = DornaMovement.DEFAULT_JOINT_SPEED
elif path == "line":
speed = DornaMovement.DEFAULT_XYZ_SPEED
if list(cmd_dict.keys())[0] in self.joint_names:
self.move_joints(path=path,
movement=DornaMovement.RELATIVE,
speed=speed,
joint_angles=cmd_dict)
elif list(cmd_dict.keys())[0] in self.axis_names:
self.move_xyzab(path=path,
movement=DornaMovement.RELATIVE,
speed=speed,
xyzab=cmd_dict)
def move_xyzab(self, path, movement, speed, xyzab, fulfill=True):
'''
path = 'joint' or 'line'\n
movement = relative or global\n
speed = units/second\n
xyzab = can be a list of all positions or a dict of single direction
'''
try:
if type(xyzab) == list and len(xyzab) == 5:
cmd_dict = dict(zip(self.axis_names, xyzab))
elif type(xyzab) == dict:
cmd_dict = xyzab
pass
else:
raise TypeError
except:
raise
parameters = dorna_cmd_utils.get_move_msg(path=path,
movement=movement,
speed=speed,
cmd_dict=cmd_dict)
cmd = dorna_cmd_utils.get_play_msg(command="move",
parameters=parameters,
fulfill=fulfill)
logger.info(cmd)
self.play(cmd)
def move_joints(self, path, movement, speed, joint_angles, fulfill=True):
'''
path = 'joint' or 'line'\n
movement = relative or global\n
speed = units/second\n
joint_angles = can be a list of all angles or a dict of some angles
'''
try:
if type(joint_angles) == list and len(joint_angles) == 5:
cmd_dict = dict(zip(self.joint_names, joint_angles))
# print(cmd_dict)
elif type(joint_angles) == dict:
cmd_dict = joint_angles
else:
raise TypeError
except:
raise
parameters = dorna_cmd_utils.get_move_msg(path=path,
movement=movement,
speed=speed,
cmd_dict=cmd_dict)
cmd = dorna_cmd_utils.get_play_msg(command="move",
parameters=parameters,
fulfill=fulfill)
logger.info(cmd)
self.play(cmd)
def move_to_home(self):
'''
Moves Dorna to default home position at default speed
'''
self.move_joints(path="joint",
movement=DornaMovement.GLOBAL,
speed=DornaMovement.DEFAULT_JOINT_SPEED,
joint_angles=self._default_home)
def move_to_zeropose(self):
'''
Moves Dorna to joint angles all 0
'''
cmd_dict = dict.fromkeys(self.joint_names, 0)
self.move_joints(path="joint",
movement=DornaMovement.GLOBAL,
speed=DornaMovement.DEFAULT_JOINT_SPEED,
joint_angles=cmd_dict)
def pause(self):
'''
Pauses Dorna command queue
'''
self._robot.pause()
def play(self, path):
'''
Adds path to command queue
'''
logger.info('Playing Path...')
try:
self._robot.play(path, append=True)
except KeyboardInterrupt:
self.halt()
pass
def set_joint(self, joint_dict=None):
'''
Sets values for motor angle
'''
if joint_dict:
self._robot.set_joint(joint_dict)
else:
logger.debug(self._default_home)
self._robot.set_joint(self._default_home)
def terminate(self):
'''
Kills threads in Dorna
'''
self._robot.terminate()
def xyz_to_joint(self, xyzab):
'''
Checks if xyz position is in workspace\n
If true then returns inverse kinematics
'''
if self._robot._config["unit"]["length"] == "mm":
xyzab[0:3] = [self._robot._mm_to_inch(xyz) for xyz in xyzab[0:3]]
joint_response_dict = self._robot._xyz_to_joint(np.asarray(xyzab))
if joint_response_dict['status'] == 0:
logger.info("Can achieve xyz position and safe")
return joint_response_dict['joint']
elif joint_response_dict['status'] == 1:
logger.info(
"Can achieve xyz position but NOT safe, USE WITH CAUTION")
return joint_response_dict['joint']
elif joint_response_dict['status'] == 2:
logger.error("Cannot achieve xyz position")
return None
####################################
#### Get information from robot ####
####################################
def get_cartesian_position(self):
'''
Get cartesian postion of end effector\n
Return in units[mm, inch] set by params
'''
pos = json.loads(self._robot.position("xyz"))
return pos
def get_connection_status(self):
'''
Get connection statues
'''
status = json.loads(self._robot.device())
return status["connection"] == DornaConnection.CONNECTED
def get_homed_status(self):
'''
Returns if stepper motors calibrated
'''
homed = json.loads(self._robot.homed())
return homed
def get_joint_angles(self, units='deg'):
'''
Returns motor joint angles
'''
joints = json.loads(self._robot.position("joint"))
if units == 'rad':
joints = [joint * np.pi / 180. for joint in joints]
return joints
def get_state(self):
'''
Returns if robot is moving
'''
state = json.loads(self._robot.device())['state']
return state
def get_param(self, param_name):
'''
Gets value of param from param_list
'''
if param_name in self.params:
if param_name == 'calibrate':
logger.warn('Dorna param calibrate is only a setting function')
return
if hasattr(self._robot, param_name):
call = getattr(self._robot, param_name, None)
response = json.loads(call())
# logger.info("Get robot param: "+str(response))
return response
else:
logger.warn(
'Dorna does not have method: {}'.format(param_name))
else:
logger.warn('[{}] not in param list'.format(param_name))
def set_param(self, param_name, value):
'''
Sets value of param from param_list
'''
if param_name in self.params:
attr = 'set_' + param_name
if hasattr(self._robot, attr):
call = getattr(self._robot, attr, None)
response = call(value)
logger.info("Set robot param: " + str(response))
else:
logger.warn('Dorna does not have method: {}'.format(attr))
else:
logger.warn("[{}] not in param list".format(param_name))
class SpectroAlign(object):
rest_cmd = {"command" : "move", "prm":{"movement" : 0, "path": "joint","speed":1000, "j0" : 0. , "j1" : 145., "j2" : -90, "j3" : 0.0 , "j4" : 0.0}}
angle_cmd = {"command" : "move" , "prm" : {"movement" : 0 , "path" : "joint", "a" : -90, "b": 0}}
start_cmd = {"command" : "move" , "prm" : {"movement" : 0 , "path" : "joint", "speed" : 1000, "j0" : -7 , "j1" : 50., "j2" : -50.}}
vertical_cmd = {"command" : "move", "prm":{"movement" : 0, "path": "joint","speed": 500, "j0" : 0. , "j1" : 90., "j2" : 0., "j3" : 0.0 , "j4" : 0.0}}
reset_j0 = {"command" : "move", "prm":{"movement" : 0, "path": "joint","speed":1000 ,"j0" : 0. }}
def __init__(self,robot = None ,center = None,rel_pos = [0.,0.,0.,0.,0.], j0_offset = 0.,
data_dir = None, laser = None):
"""
This creates the python object, we can specify initial conditions for the robot
Typically, we assume we're starting from the rest position, pointed away from the table.
"""
if center == None:
self.center = [0.,0.,0.,0.,0.]
self.center_set = False
else:
self.center = center
self.center_set = True
if data_dir == None:
self.data_dir = "/home/fireball2/SpectroAlign/" + datetime.now().strftime("%y%m%d") +'/' #'/Users/ignacio/' + datetime.now().strftime("%y%m%d") +'/'
else:
self.data_dir = data_dir
self.log_str = self.data_dir + "logfile"
#self.logfile = open(self.data_dir + "logfile", 'a')
self.rel_pos = rel_pos
self.j0_offset = j0_offset
if robot == None:
self.robot = Dorna()
else:
self.robot = robot
self.imno = 0
if laser == None:
self.laser = None
self.laser_set = False
else:
self.laser = laser
self.laser_set = True
self.z_laser = 0.
def __repr__(self):
return self.robot.device()
####### Laser readout tools
def laser_read(self, set_target = False):
'''
launches laser_main program and reads measurement
output: (float) laser distance measured [mm]
Set target records value to Z attribute
'''
if (self.laser_set):
out = self.laser.measure()
if set_target:
self.z_laser = out
return out
else:
print("Laser not configured")
return None
'''
cd_cmd = "cd /home/fireball2/laser"
subprocess.run(cd_cmd)
laser_cmd = "./laser_main"
result = subprocess.run(laser_cmd, stdout = PIPE)
print(result.stdout.decode())
i = result.rfind("M0,") + 3
if i == -1:
print("measurement not found")
return None
j = result[i:].find[","]
distance = float(result[i:i+j])
return distance '''
def setup_laser(self):
'''
Open serial connection to laser, creates laser_serial object
'''
if (self.laser_set):
print("Laser port already open")
return None
self.laser = LaserIO()
self.laser_set = True
self.z_laser = self.laser.measure()
return None
'''
readout = self.laser_read()
if readout == None:
print("Laser not found")
return None
else:
self.z_laser = readout
self.laser = True
return None'''
def z_correct(self):
'''
adjusts z position and relative coordinates to account for drift
'''
if self.laser_set == True:
readout = self.laser_read()
z_offset = self.z_laser - readout
print('Laser offset correction: %s mm' % str(z_offset))
self.xyz_v2(z = z_offset)
self.set_center()
return None
else:
print("Laser not configured!")
return None
def connect(self,port = "/dev/ttyACM0"): ##"/dev/cu.usbmodem14101" for mac
"""
connects the robot arm.
input:
port: (string) port address for robot
"""
self.robot.connect(port)
return self.robot.device()
def set_j3(self):
"""
moves j3/j4 to zero position, starting from fiber holder
in the calibration position (touching the top of the arm)
"""
self.robot.set_joint({"j3" : 152.0 , "j4" : 0.0})
self.joint_v2(j3 = -152)
return None
def checkout(self):
"""
Disconnect robot and terminate robot dorna object
"""
self.robot.disconnect()
self.robot.terminate()
if self.laser_set:
self.laser.close()
return self.robot.device()
def calibrate(self,position = [0.,90.,0.,0.,0.]):
"""
Defines current joint position of the robot in the Dorna joint system.
defaults to calibrating on the stretched upwards position
input:
position: (list of floats) [j0,j1,j2,j3,j4] in degrees
returns:
new joint values
"""
self.completion()
self.robot.set_joint(position)
self.robot.save_config()
return self.robot.position()
def log_line(self, line):
"""
Write line to logfile
"""
logfile = open(self.log_str, 'a')
logfile.write(line)
logfile.close()
return None
#### Robot motion and dorna parameter methods
def play(self, cmd):
"""
passes Dorna command to embeded Dorna object
input:
cmd: (dict) in Dorna format for motions
output:
dorna play command output
"""
output = self.robot.play(cmd)
self.completion()
self.find_rel()
return output
def home(self, joint):
"""
passes joint name to be homed
input:
joint: (string) name of joint
output:
dorna play command output
"""
output = self.robot.home(joint)
return output
def set_center(self,coords = None):
"""
Sets center of field coordinates at current robot position, or at specified coords.
input:
coords: (list of floats) x,y,z,a,b position of arm in dorna coordinates
output:
(list of floats) position of new center in dorna xyzab format
"""
if coords == None:
self.center = json.loads(self.robot.position("xyz"))
else:
self.center = coords
#print("Fiber Center :" + str (self.center))
#self.log_line("\nSet Fiber Center [mm]: " + str(self.center))
self.center_set = True
return self.center
def find_rel(self):
"""
Calculates and updates relative position to center of field
"""
robot_pos = json.loads(self.robot.position("xyz"))
x = robot_pos[0] - self.center[0]
y = robot_pos[1] - self.center[1]
z = robot_pos[2] - self.center[2]
a = robot_pos[3]
b = robot_pos[4]
self.rel_pos = [x,y,z,a,b]
#print("relative position : " + str(self.rel_pos))
return self.rel_pos
def to_center(self):
"""
Returns the robot to the center of field as recorded.
"""
if self.center_set:
center_cmd = {"command": "move", "prm": {"movement": 0, "path": "line" , "xyz" : self.center }}
self.play(center_cmd)
else:
print("Center coordinate has not been set")
return None
def to_xyz(self,coords):
"""
Moves robot to given position in xyz
"""
motion_cmd = {"command": "move", "prm": {"movement": 0, "path": "line" , "xyz" : coords}}
self.play(motion_cmd)
return None
def to_joint(self,coords):
"""
Moves robot to given position in joint coord.
"""
motion_cmd = {"command": "move", "prm": {"movement": 0, "path": "line" , "joint" : coords}}
self.play(motion_cmd)
return None
def ax_rotation(self,offset):
"""rotates around z and redefines x, y coordinates of the robot.
j0 offset: degrees in counterclockwise direction looking from above.
"""
self.play(SpectroAlign.rest_cmd)
self.play({"command" : "move", "prm":{"movement" : 0, "speed":1000, "path": "joint", "j0" : offset }})
self.j0_offset += offset
self.completion()
self.robot.set_joint({"j0" : 0 })
return None
def completion(self):
"""
Wait until previous robot motion is completed.
"""
status = self.robot.device()
parsed_status = json.loads(status)
while parsed_status['state'] != 0:
time.sleep(0.5)
status = self.robot.device()
parsed_status = json.loads(status)
#print("Command is done. On to the next!")
return None
def xyz_v2(self,x = None ,y = None, z = None, speed = 2500, movement = 1, y_angle = True):
"""
Moves the fiber in x y z directions
input:
speed: (float) unit length/minute
dx, dy, dz "x", "y" or "z" for direction of motion
value: (float)
"""
self.completion()
start_joint = json.loads(self.robot.position(space = "joint"))
start_xyz = json.loads(self.robot.position(space = "xyz"))
input_xyz = [x,y,z]
#print(input_xyz)
if movement == 0:
for i in range(3):
if input_xyz[i] == None:
input_xyz[i] = start_xyz[i]
elif movement == 1:
for i in range(3):
if input_xyz[i] == None:
input_xyz[i] = 0.
else:
print("Invalid movement parameter!")
return None
#print(input_xyz)
motion = {"command" : "move" , "prm" : {"movement" : movement ,"speed": speed,
"path" : "line", "x" : input_xyz[0], "y" : input_xyz[1],"z" : input_xyz[2]}}
self.play(motion)
self.completion()
end_joint = json.loads(self.robot.position(space = "joint"))
base_twist = (start_joint[0] - end_joint[0])
if base_twist != 0:
line_up = {"command" : "move" , "prm" : {"movement" : 0, "path" : "joint", "j4" : - end_joint[0] ,"speed" : 2000 }}
self.play(line_up)
self.completion()
return None
def joint_v2(self, j0 = None, j1 = None, j2 = None, j3 = None, j4 = None, speed = 1500, movement = 1):
"""
input:
jx: (float) value along direction of motion
value: (float) angle in degrees
speed: (float) degrees/min
movement: 1 for relative to current location, 0 for absolute cooridnates
"""
self.completion()
input_joint = [j0,j1,j2,j3,j4]
start_joint = json.loads(self.robot.position(space = "joint"))
print("joints: " + str(input_joint))
if movement == 0:
for i in range(5):
if input_joint[i] == None:
input_joint[i] = start_joint[i]
elif movement == 1:
for i in range(5):
if input_joint[i] == None:
input_joint[i] = 0.0
else:
print("Invalid movement parameter!")
return None
motion = {"command" : "move" , "prm" : {"movement" : movement ,"speed": speed,
"path" : "joint", "j0" :input_joint[0] , "j1" :input_joint[1], "j2" :input_joint[2], "j3" :input_joint[3], "j4" :input_joint[4]}}
self.play(motion)
self.completion()
self.find_rel()
return None
def xyz_motion(self,coord,value,speed = 2500):
"""
Moves the fiber in x y z directions
input:
speed: (float) unit length/minute
coord: (string) "x", "y" or "z" for direction of motion
value: (float)
"""
self.completion()
if coord == "x":
grid_x = {"command" : "move" , "prm" : {"movement" : 1 ,"speed": speed, "path" : "line", "x" : value}}
self.play(grid_x)
elif coord == "z":
grid_z = {"command" : "move" , "prm" : {"movement" : 1 ,"speed": speed, "path" : "line", "z" : value}}
self.play(grid_z)
elif coord == "y":
grid_y = {"command" : "move" , "prm" : {"movement" : 1 ,"speed": speed, "path" : "line", "y" : value}}
self.play(grid_y)
else:
print("Invalid coordinate command")
self.completion()
self.find_rel()
return None
def joint_motion(self,coord,value,speed = 1500 ,movement = 1):
"""
Shorthand function to move robot joints
input:
coord: (string) "j0", "j1", "j2", "j3" or "j4" for direction of motion
value: (float) angle in degrees
speed: (float) degrees/min
movement: 1 for relative, 0 for absolute
"""
self.completion()
if coord == "j0":
cmd = {"command" : "move" , "prm" : {"movement" : movement ,"speed" : speed, "path" : "joint", "j0" : value}}
self.play(cmd)
elif coord == "j1":
cmd = {"command" : "move" , "prm" : {"movement" : movement ,"speed" : speed, "path" : "joint", "j1" : value}}
self.play(cmd)
elif coord == "j2":
cmd = {"command" : "move" , "prm" : {"movement" : movement,"speed" : speed, "path" : "joint", "j2" : value}}
self.play(cmd)
elif coord == "j3":
cmd = {"command" : "move" , "prm" : {"movement" : movement,"speed" : speed, "path" : "joint", "j3" : value}}
self.play(cmd)
elif coord == "j4":
cmd = {"command" : "move" , "prm" : {"movement" : movement,"speed" : speed, "path" : "joint", "j4" : value}}
self.play(cmd)
else:
print("Invalid coordinate command")
self.completion()
self.find_rel()
return None
def find_pos(self):
"""
returns position in dorna xyz coordinates as list
output:
(list of floats) xyzab coordinates
"""
position = json.loads(self.robot.position("xyz"))
print("x,y,z,a,b =" + str(position))
return position
### Nuvu control methods
def set_log(self):
"""
Creates log file for image taking, sets up camserver image path
output
(textio) pointer to file.
"""
#data_dir = '/home/fireball2/SpectroAlign/' + datetime.now().strftime("%y%m%d") +'/'
command = "mkdir -p " + self.data_dir
p= Popen(command, shell=True, stdin=PIPE, stdout=PIPE, stderr=STDOUT, close_fds=True)
#logfile = open(data_dir+'logfile', 'a')
#command = 'cam path ' + self.data_dir
#p = Popen(command, shell=True, stdin=PIPE, stdout=PIPE, stderr=STDOUT, close_fds=True)
#output = p.stdout.read()
self.log_line('Data directory=' + self.data_dir + "\n")
return None
def expose(self,exptime, burst = 1):
# Take images
command = 'cam imno'
#p = Popen(command, shell=True, stdin=PIPE, stdout=PIPE, stderr=STDOUT, close_fds=True)
imno1 = self.imno
#self.logfile.write('\nimage%06d.fits' % int(imno1))
command0 = 'cam burst='+str(burst)
print(command0)
# Run command
#p = Popen(command0, shell=True, stdin=PIPE, stdout=PIPE, stderr=STDOUT, close_fds=True)
# Write to the log file
bpar = str(burst)
#bpar = int(p.stdout.read().decode())
#self.logfile.write('\nBurst param= %s' % str(bpar))
#subprocess.call(command0,shell=True)
# Set the exposure time
command1 = 'cam exptime='+str(exptime)
print(command1)
# Run command
#p = Popen(command1, shell=True, stdin=PIPE, stdout=PIPE, stderr=STDOUT, close_fds=True)
expout = "exptime"
#expout = float(p.stdout.read().decode())
#self.logfile.write('\nExposure time= %s' % str(expout))
#subprocess.call(command1,shell=True)
# command2 = './cam emgain='+str(emgain)
# print command2
# Run command
# p = Popen(command2, shell=True, stdin=PIPE, stdout=PIPE, stderr=STDOUT, close_fds=True)
# Write to the log file
# output = p.stdout.read()
# logfile.write('EM gain= %s' % str(output))
#subprocess.call(command2,shell=True)
command3 = 'cam expose'
print(command3)
# Run command
#p = Popen(command3, shell=True, stdin=PIPE, stdout=PIPE, stderr=STDOUT, close_fds=True)
# Write to the log file
for i in range(burst):
self.log_line('\nimage%06d.fits' % (imno1+i))
self.log_line('\t%s' % str(expout))
self.log_line('\t%s' % str(bpar))
self.log_line('\t%s' % str(burst))
self.log_line('\t%s' % str(self.rel_pos[0]))
self.log_line('\t%s' % str(self.rel_pos[1]))
self.log_line('\t%s' % str(self.rel_pos[2]))
if self.laser_set:
self.log_line('\t%s' % str(self.laser_read()))
time.sleep(exptime*burst)
self.imno += burst
return None
### Combined routines (exposures + motion)
def focus_test(self, steps=10, dz = 0.5 , exptime = 1., burst =1, dx0 = 0., dy0 = 0.,dz0 = 0.):
"""
Takes through focus image bursts as the robot moves upwards in z.
input:
steps: (int) number changes in position
dz: (float) separation between steps in mm
exptime = (float) exposure in seconds
burst = (int) number of images per exposure
dx0 = (float) displacement from original x position in mm
dy0 = (float) displacement from original y position in mm
dz0 = (float) displacement from original z position in mm
"""
print("Sweeping focus on Z")
start_coord = json.loads(self.robot.position("xyz"))
return_cmd = {"command": "move", "prm": {"movement": 0, "path": "line" , "xyz" : start_coord}}
if dx0 != 0.:
self.xyz_motion( "x", dx0)
if dy0 != 0.:
self.xyz_motion("y", dy0)
if dz0 != 0.:
self.xyz_motion("z",dz0)
for i in range(steps):
z_cmd = {"command" : "move" , "prm" : {"movement" : 0 , "path" : "line", "z" : start_coord[2]+(dz*i)}}
self.play(z_cmd)
self.completion()
print("z = " + str(start_coord[2]+dz))
print('Relative position [mm] %s' % str(self.rel_pos))
if self.laser_set:
ext_z = self.laser_read()
self.expose(exptime,burst)
print("Sweep complete, returning to start position")
self.play(return_cmd)
self.completion()
return None
def repeat_grid(self,gx = 5, gy = 5):
"""
Grid motions to test repeatability
"""
start_coord = json.loads(self.robot.position("xyz"))
return_cmd = {"command": "move", "prm": {"movement": 0, "path": "line" , "xyz" : start_coord}}
self.laser_read(set_target = True)
print("moving in grid")
for i in range(3):
for j in range(3):
self.xyz_v2(x = start_coord[0]+((i-2)*gx), y = start_coord[1]+((j-2)*gy), movement = 0)
self.focus_test(steps = 5)
self.xyz_v2(x = start_coord[0], y = start_coord[1], movement = 0)
self.z_correct()
self.play(return_cmd)
return None