import urx, math, math3d, time class UR5Exception(Exception): pass class DemoController(object): """Implements control of the UR5 in cylinder coordinates (corresponding to attached table). -Checks bounds of the arc section -Methods for placing blocks """ def __init__(self, config): """Initializes the controller with parameters specified in config""" self.debug = config.DEBUG try: self.robot = urx.Robot(config.UR5_IP, useRTInterface=config.USE_FORCE_MONITOR) #TODO or hostname except Exception, e: raise UR5Exception('Robot initiation failed: %s' % str(e)) # Cylinder coordinate system params self.quadrant = config.TABLE_QUADRANT self.phi = self.quadrant*math.pi/2 self.cyl_offset = config.TABLE_ORIGO_OFFSET self.cyl_ox = self.cyl_offset*math.cos(self.phi) self.cyl_oy = self.cyl_offset*math.sin(self.phi) # Home pose params self.j_home = config.JOINTS_HOME self.r_home = config.R_HOME self.theta_home = config.THETA_HOME self.z_home = config.Z_HOME # Force monitor params self.force_mon = config.USE_FORCE_MONITOR self.force_constraint = config.FORCE_CONSTRAINT self.t_force = config.T_FORCE_VIOLATION # Manual control velocity params self.vel = config.VEL self.vel_z = config.VEL_Z self.acc = config.ACC self.stop_acc = config.STOP_ACC self.t_chcmd = config.T_DIR_CHANGE_COOLDOWN self.prev_vec = (0,0,0) self.t_ch = 0 # Manual control constraints self.r_min = config.R_MIN self.r_max = config.R_MAX self.theta_min = config.THETA_MIN self.theta_max = config.THETA_MAX self.z_min = config.Z_MIN self.z_max = config.Z_MAX # Block move params self.block_dim = config.BLOCK_DIM self.r_lvl0 = config.R_LVL0 self.z_lvl0 = config.Z_LVL0 self.r_pick_init = config.R_STARTPICK_OFFSET self.theta_pick_init = config.THETA_STARTPICK_OFFSET self.z_pick_init = config.Z_STARTPICK_OFFSET self.r_pick_end = config.R_ENDPICK_OFFSET self.theta_pick_end = config.THETA_ENDPICK_OFFSET self.z_pick_end = config.Z_ENDPICK_OFFSET def move_to_home_pose(self): """Move to "home" configuration. The resulting pose has correct orientation.""" try: self.robot.movej(self.j_home, acc=0.1, vel=0.5, radius=0, wait=True, relative=False) pose = self.robot.getl() pose[0] = self.r_home*math.cos(self.phi+self.theta_home) + self.cyl_ox pose[1] = self.r_home*math.sin(self.phi+self.theta_home) + self.cyl_oy pose[2] = self.z_home self.robot.movel(pose, acc=0.1, vel=0.2, radius=0, wait=True, relative=False) self.current_cyl = (self.r_home, self.theta_home, self.z_home) if self.debug: print "Initial cylinder coords: %s" % str([('%.2f' % i) for i in self.current_cyl]) except Exception, e: raise UR5Exception('Move to home configuration failed: %s' % str(e)) def calibrate_cylinder_sys(self): """Calibrate the reference cylinder coordinate system.""" # Move to reference coordinate system base self.move_to_home_pose() # Set reference coordinate system parameters csys = math3d.Transform() csys.orient.rotate_zb(self.phi) self.robot.set_csys("csys", csys) self.trans_base = self.robot.get_transform() self.cyl_ox = self.cyl_offset self.cyl_oy = 0 def set_cylinder_coords(self): p = self.robot.getl() x = p[0] - self.cyl_ox y = p[1] - self.cyl_oy z = p[2] r = (x**2 + y**2)**0.5 theta = math.atan2(y, x) self.current_cyl = (r, theta, z) def cylinder2cartesian(self, r, theta, z): """Translate table cylinder coordinates to reference cartesian coordinates.""" trans = math3d.Transform(self.trans_base) #deep copy trans.orient.rotate_zb(theta) trans.pos = math3d.Vector(r*math.cos(theta) + self.cyl_ox, r*math.sin(theta) + self.cyl_oy, z) return trans.pose_vector.tolist() def exec_move(self, move, wait=False): """Execute a linear move. move_list = pose + [acc, vel, radius]""" prog = "movel(p[{},{},{},{},{},{}], a={}, v={}, r={})".format(*move) self.robot.send_program(prog) if wait: self.robot.wait_for_move() def exec_moves(self, move_list, wait=False): """Executes a list of linear moves. move_list = pose + [acc, vel, radius]""" header = "def myProg():\n" end = "end\n" template = "movel(p[{},{},{},{},{},{}], a={}, v={}, r={})\n" prog = header + ''.join([template.format(*m) for m in move_list]) + end self.robot.send_program(prog) if wait: self.robot.wait_for_move() def movec(self, r, theta, z, wait=False): """Circular move - uses URScript built-in function. Unfortunately, this doesn't work very well: The tool orientation at the target pose is not right. URScript specifies that orientation in pose_via is not considered, however, it SHOULD consider the orientation of the end pose. """ curr_theta = self.current_cyl[1] pose_via = self.cylinder2cartesian(r, curr_theta+(theta-curr_theta)/2, z) pose = self.cylinder2cartesian(r, theta, z) self.robot.movec(pose_via, pose, acc=self.acc, vel=self.vel, radius=0, wait=wait) def linearize_arc(self, r, theta, z, resolution=math.pi/41, blend=0.03): """movec is unreliable and does not finish in the correct pose. We work around this by linearizing the arc into segments with length given in resolution, and use movels with blending. IMPORTANT: blending radius have to be smaller than the angular resolution! If not, the robot will not finish the last move, because it is within the target """ move_list = [] step = resolution curr_r, curr_theta, curr_z = self.current_cyl dtheta = theta - curr_theta segments = max(int(round(abs(dtheta)/step)), 1) theta_incr = dtheta/segments r_incr = (r-curr_r)/segments z_incr = (z-curr_z)/segments for i in range(1, segments): pose = self.cylinder2cartesian(curr_r + i*r_incr, curr_theta + i*theta_incr, curr_z + i*z_incr) move_list.append(pose + [self.acc, self.vel, blend]) move_list.append(self.cylinder2cartesian(r, theta, z) + [self.acc, self.vel, 0]) #dont blend last return move_list def movec_hax(self, r, theta, z, blend_last=0, wait=False): """movec with linearized arc.""" # manual acc/vel move_list = self.linearize_arc(r, theta, z) if self.debug: print "move list for movec_hax" for p in move_list: print [('%.3f' % i) for i in p] self.exec_moves(move_list, wait=wait) def move(self, vec, t=0): """Move in vec direction. Assumes that |v| != 0""" vr, vtheta, vz = vec r, theta, z = self.current_cyl if vtheta == 0: # linear move if vz == 0: move = else: # arc move if vr != 0 or (vr != 0 and vtheta != 0): if vr != 0: self.movel(r+vr, theta, z, wait=False) elif vtheta != 0: self.movec_hax(r, theta+vtheta, z, wait=False) elif vz != 0: self.movel(r, theta, z+vz, wait=False) if t: time.sleep(t) self.stopl(acc=self.chcmd_decel) def update(self, vec, dt): """Update movements based on vec (and dt?)""" force = 10 # dummy if self.force_mon and force > self.force_constraint: self.move([i*-1 for i in vec], t=self.ft) self.set_current_cyl() return self.set_current_cyl() vr, vtheta, vz = vec r, theta, z = self.current_cyl # move? if sum(map(abs, vec)) == 0: if vec != self.prev_vec: self.robot.stopl(acc=self.chcmd_decel) prev_vec = (0,0,0) return # check bounds rnext = r + (vr*self.vel*dt) if rnext < self.r_min or rnext > self.r_max: vr = 0 thetanext = theta + (vtheta*self.vel*dt) # this is angular speed, but the diff is not significant if thetanext < self.theta_min or thetanext > self.theta_max: vtheta = 0 znext = z + (vz*self.vel*dt) if znext < self.z_min or znext > self.z_max: vz = 0 vec = (vr, vtheta, vz) # command change if vec != self.prev_vec: # from stand still if sum(map(abs, self.prev_vec)) == 0: self.move(vec) self.prev_vec = vec self.t_ch = 0 # from another command elif not self.t_ch: self.t_ch = time.time() self.robot.stopl(acc=self.chcmd_decel) elif time.time() - self.t_ch > self.t_chcmd: self.move(vec) self.prev_vec = vec self.t_ch = 0 def is_looping(self, dt, threshold=0.001): """Polls the robot for change in pose vector. Blocks for dt seconds.""" pose0 = self.robot.getl() time.sleep(dt) pose1 = self.robot.getl() v = [pose1[i]-pose0[i] for i in range(len(pose0))] if self.debug: _sum = sum(map(abs, v)) print "Velocity vector, dt=%.2f: %s. SUM %.3f" % (dt, str([('%.2f' % i) for i in v]), _sum) return sum(map(abs, v)) > threshold def blocklevel2move(self, r_lvl, theta, z_lvl, acc=0.5, vel=0.1, r=0): # loop acc/vel return self.cylinder2pose(self.r_lvl0 - r_lvl*self.block_dim, theta, self.z_lvl0 + z_lvl*self.block_dim) def pick_block(self, r_lvl, theta, z_lvl, acc, vel): """This function concatenates 3 moves: 1. Move the tool in front of the block. 2. Move the tool into the hole. 3. Lift the block. Here, we assume that the tool is nearby the block. Bringing the tool towards the block is not the responsibility of this function. r_lvl starts at 0, which correspond to the outer edge of the table. z_lvl starts at 0, which corrsepond to table level. """ # loop acc/vel r_target = self.r_lvl0 - r_lvl*self.block_dim z_target = self.z_lvl0 + z_lvl*self.block_dim p1 = self.cylinder2pose(r_target - self.r_bmargin, theta - self.theta_bmargin, z_target - self.z_bmargin) p2 = self.cylinder2pose(r_target, theta, z_target) p3 = self.cylinder2pose(r_target + self.r_boffset, theta + self.theta_boffset, z_target + self.z_boffset) move_list = [ p1 + [self.acc, vel, 0.005], p2 + [self.acc, vel, 0], p3 + [self.acc, vel, 0.01], ] return move_list def place_block(self, r_lvl, theta, z_lvl, acc, vel): """Reverse move of pick_block.""" # loop acc/vel moves = self.pick_block(r_lvl, theta, z_lvl, vel) moves.reverse() return moves def construct_moves(self, task_list, acc, vel): """Construct moves from a list of tasks""" pass # wait with implementing this def set_freedrive(self, mode): self.robot.set_freedrive(mode) def cleanup(self): self.robot.cleanup()