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 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))