diff --git a/config.py b/config.py index 742f23e..17d94d3 100644 --- a/config.py +++ b/config.py @@ -2,71 +2,136 @@ import math DEBUG = True + # GUI +# +# Frame rates should be high enough to poll keys efficiently. SCREEN_DIM = (800, 600) FONTSIZE = 24 -BG_COL = (255,255,255) #white -FONT_COL = (0,0,0) #black -MENU_FPS = 15 # enough to poll keys efficiently -CTR_FPS = 60 +BG_COL = (255,255,255) +FONT_COL = (0,0,0) +MENU_FPS = 15 +CTR_FPS = 60 #manual control -# IP of the robot. + + +# NETWORK +# +# The IP address can be found in the PolyScope interface (tablet) of the robot. +# SETUP Robot -> Setup NETWORK (requires password) -> IP address UR5_IP = "129.241.187.119" +UR5_HOSTNAME = 'ur-2012208984' #requires dns. -# Enables force constraints on the tool. Note: Setting this to "True" involves -# polling the UR5 through a real-time monitor that runs at 125Hz. If the controller -# runs slow, try disabling this first. + + +# BLOCK DIMENSION +# +# Blocks are cubic and the hole is centered. +BLOCK_DIM = 0.0975 #not exactly 10cm + + + +# TABLE CONFIGURATION +# +# The table configuration specifies parameters for the cylinder coordinate system. +# All table parameters are relative to the robots base coordinate system. +# +# TABLE_QUADRANT was added because the UR5's kinetmatics computations are dependent +# on its own base coordinate system. For instance, quadrant 3.0 results in joint +# violations in the original setup. Quadrant 0.0 corresponds to the table center +# along the base x-axis, quadrant 1.0 corresponds to the base y-axis, etc. +# Quardant 0 is recommended. +TABLE_QUADRANT = 0 #[0..4) +TABLE_ORIGO_OFFSET = -0.25 #offset from origo along base x-axis +TABLE_Z = -0.336 #this should be fine-tuned on each setup +TABLE_ARC = math.pi/2 + + + +# HOME JOINT CONFIGURATION +# +# This is the joint config for the robot's initial position. +# We need this also expressed in joint space to ensure that the robot does not +# collide with itself when moving to the base configuration, and to ensure +# the correct tool orientation when calibrating the reference cylinder +# coordinate system. +# +# Change joints 2-6 in QUADRANT_JOINTS_HOME to tune the tool orientation. +R_HOME = 0.5 +THETA_HOME = 0 +Z_HOME = TABLE_Z + 2*BLOCK_DIM +JOINTS_HOME = [TABLE_QUADRANT*math.pi/2, + -3*math.pi/4, + -3*math.pi/4, + -math.pi/2, + -math.pi/2, + 49*math.pi/50] + + + +# FORCE MONITOR +# +# Enables force constraints on the tool (for manual control). +# When the force exceeds FORCE_CONSTRAINT (newton), the robot move in the direction +# opposite to the current command in T_FORCE_VIOLATION seconds. +# Note: Setting this to "True" involves polling the UR5 through a real-time +# monitor that runs at 125Hz. If the controller runs slow, try disabling this. USE_FORCE_MONITOR = False FORCE_CONSTRAINT = 60 -FORCE_T = 0.1 +T_FORCE_VIOLATION = 0.1 -# Control parameters. -VELOCITY = 0.1 -ACCELERATION = 0.5 -DECELERATION = 0.5 -CHCMD_DECEL = 1.0 -CHCMD_T = 0.1 # cooldown between commands to smooth out the change in motion -# Loop parameters -DEFAULT_LOOP_SPEED = 0.1 -LOOP_SPEED_MAX = 0.7 -LOOP_SPEED_MIN = 0.05 -# Block parameters -BLOCK_DIM = 0.0975 #TODO measure +# MANUAL CONTROL +# +# Parameters that specifies velocity curve and constraints of manual control. +# Note that the angular velocity is also [m/s], not [rad/s], to ensure that the robot +# moves with constant speed regardless of direction in the polar plane. +# IMPORTANT: VEL_Z should be tuned to Z_MIN to avoid smashing into the table. +VEL = 0.1 #[m/s] +VEL_Z = 0.05 #[m/s] +ACC = 1.0 #[m/s^2] +STOP_ACC = 1.0 #aka deceleration +T_DIR_CHANGE_COOLDOWN = 0.1 -# Table cylinder parameters -TABLE_QUADRANT = 0 # In range [0..4). Quadrant 0 is along the x-axis of the robot. -TABLE_ORIGO_OFFSET = -0.25 # TODO measure -Z_TABLE = -0.336 -R_MIN = 0.5 # relative to TABLE center -R_MAX = 0.8 # relative to TABLE center -THETA_MIN= -math.pi/4 # table edge left -THETA_MAX = math.pi/4 # table edge right -Z_MIN = Z_TABLE + BLOCK_DIM/2 -Z_MAX = Z_MIN + 5*BLOCK_DIM +# Constraints are relative to the cylinder coordinate system +R_MIN = 0.5 +R_MAX = 0.8 +THETA_MIN= -TABLE_ARC/2 +THETA_MAX = TABLE_ARC/2 +Z_MIN = TABLE_Z + 0.6*BLOCK_DIM +Z_MAX = TABLE_Z + 4.5*BLOCK_DIM #4 blocks high -# Block move parameters -R_LVL0 = 0.7 # tip of tool at table edge -Z_LVL0 = Z_MIN -THETA_EDGE_LEFT = -math.pi/4 + math.pi/68 + + +# AUTO/LOOP CONTROL +# +# When looping, the robot use the same velocity for r, theta and z. +# Also, since this is all planned motions, we dont need to use stopl(). +# This means that the trapezoidal velocity curve has the same slope at +# start and end of a motion. (acceleration=deceleration) +# +# Constraints in r and z-direction are specefied in levels to enable +# easier construction of loops. Note that the r-levels grow inwards, and +# the z-levels grow upwards. +DEFAULT_LOOP_VEL = 0.1 +LOOP_VEL_MIN = 0.05 +LOOP_VEL_MAX = 0.7 +LOOP_ACC = 0.5 + +R_LVL0 = 0.7 #tip of tool at table edge +Z_LVL0 = TABLE_Z + BLOCK_DIM/2 #tool in middle of block +THETA_EDGE_LEFT = -math.pi/4 + math.pi/68 #empirical. table is not exact THETA_EDGE_RIGHT = math.pi/4 - math.pi/68 -R_BLOCKMOVE_MARGIN = BLOCK_DIM + 0.05 # r margin before and after pick/place -THETA_BLOCKMOVE_MARGIN = 0 # theta margin before and after pick/place -Z_BLOCKMOVE_MARGIN = 0 # z margin before and after pick/place -R_BLOCKMOVE_OFFSET = -0.01 # r start/end in place/pick. lift slightly inwards to avoid collision with other blocks -THETA_BLOCKMOVE_OFFSET = 0 # theta start/end in place/pick -Z_BLOCKMOVE_OFFSET = BLOCK_DIM*0.5 # z start/end in place/pick +# Offsets when picking (and placing) blocks. +# The place-block move is the reverse of the pick-block move, +# such that the start/end offsets for pick-block are the end/start offsets +# for place-block +R_STARTPICK_OFFSET = -1.3*BLOCK_DIM +THETA_STARTPICK_OFFSET = 0 +Z_STARTPICK_OFFSET = 0 +R_ENDPICK_OFFSET = -0.01 #lift slightly inwards to avoid collision with adjacent blocks +THETA_ENDPICK_OFFSET = 0 +Z_ENDPICK_OFFSET = BLOCK_DIM*0.5 - -# Home quadrant joint configuration. -# We need this expressed in joint space to ensure that the robot does not -# collide with itself when moving to the base configuration. -# This is also for fine-tuning the tool orientation -QUADRANT_JOINT_CONFIG = [TABLE_QUADRANT*math.pi/2, - -3*math.pi/4, - -3*math.pi/4, - -math.pi/2, - -math.pi/2, - 49*math.pi/50] diff --git a/config.pyc b/config.pyc index cfecd6e..ba0cef4 100644 Binary files a/config.pyc and b/config.pyc differ diff --git a/ur5controller.py b/ur5controller.py index 03ec960..f008f72 100644 --- a/ur5controller.py +++ b/ur5controller.py @@ -20,27 +20,34 @@ class DemoController(object): except Exception, e: raise UR5Exception('Robot initiation failed: %s' % str(e)) - # Control params - self.vel = config.VELOCITY - self.acc = config.ACCELERATION - self.dec = config.DECELERATION - self.chcmd_decel = config.CHCMD_DECEL - self.t_chcmd = config.CHCMD_T - self.prev_vec = (0,0,0) - self.t_ch = 0 - - self.force_mon = config.USE_FORCE_MONITOR - self.force_constraint = config.FORCE_CONSTRAINT - self.ft = config.FORCE_T - #self.controllable = False - - # Cylinder params + # 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 @@ -48,41 +55,40 @@ class DemoController(object): self.z_min = config.Z_MIN self.z_max = config.Z_MAX - # Block params + + # Block move params self.block_dim = config.BLOCK_DIM self.r_lvl0 = config.R_LVL0 self.z_lvl0 = config.Z_LVL0 - self.r_bmargin = config.R_BLOCKMOVE_MARGIN - self.theta_bmargin = config.THETA_BLOCKMOVE_MARGIN - self.z_bmargin = config.Z_BLOCKMOVE_MARGIN - self.r_boffset = config.R_BLOCKMOVE_OFFSET - self.theta_boffset = config.THETA_BLOCKMOVE_OFFSET - self.z_boffset = config.Z_BLOCKMOVE_OFFSET - - # Home quadrant joint config - self.j_home = config.QUADRANT_JOINT_CONFIG - - def set_pose_home(self): + 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_min*math.cos(self.phi) + self.cyl_ox - pose[1] = self.r_min*math.sin(self.phi) + self.cyl_oy - pose[2] = self.z_min + 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_min, 0, self.z_min) + self.current_cyl = (self.r_home, self.theta_home, self.z_home) if self.debug: - print "initial cyl coords: %s" % str([('%.2f' % i) for i in self.current_cyl]) + 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_csys(self): + def calibrate_cylinder_sys(self): """Calibrate the reference cylinder coordinate system.""" # Move to reference coordinate system base - self.set_pose_home() + self.move_to_home_pose() # Set reference coordinate system parameters csys = math3d.Transform() csys.orient.rotate_zb(self.phi) @@ -90,29 +96,9 @@ class DemoController(object): self.trans_base = self.robot.get_transform() self.cyl_ox = self.cyl_offset self.cyl_oy = 0 - #self.controllable = True - - self.set_current_cyl() - if self.debug: - print "Current cyl coords: %s" % str([('%.2f' % i) for i in self.current_cyl]) - - def set_freedrive(self, mode): - """Set UR5 to freedrive mode.""" - self.robot.set_freedrive(mode) - - - def cylinder2pose(self, r, theta, z): - """Translate table cylinder coordinates to robot cartesian pose.""" - 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 set_current_cyl(self): + def set_cylinder_coords(self): p = self.robot.getl() x = p[0] - self.cyl_ox y = p[1] - self.cyl_oy @@ -122,14 +108,36 @@ class DemoController(object): self.current_cyl = (r, theta, z) - def movel(self, r, theta, z, wait=True): - """Linear move.""" - pose = self.cylinder2pose(r, theta, z) - self.robot.movel(pose, acc=self.acc, vel=self.vel, radius=0, wait=wait, relative=False) - 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 movec(self, r, theta, z): + 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 @@ -137,37 +145,21 @@ class DemoController(object): it SHOULD consider the orientation of the end pose. """ curr_theta = self.current_cyl[1] - pose_via = self.cylinder2pose(r, curr_theta+(theta-curr_theta)/2, z) - pose = self.cylinder2pose(r, theta, z) - self.robot.movec(pose_via, pose, acc=self.acc, vel=self.vel, radius=0, wait=True) - self.current_cyl = (r, theta, z) + 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 movels(self, move_list, wait=True): - """Concatenate several movel commands and applies a blending radius. - 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 not wait: - return None - else: - self.robot.wait_for_move() - return self.robot.getl() - - def linearize_arc(self, r, theta, z, acc=None, vel=None, blend_last=0): + 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 of a predefined angular resolution, and use + 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 = math.pi/41 + step = resolution curr_r, curr_theta, curr_z = self.current_cyl dtheta = theta - curr_theta @@ -177,85 +169,40 @@ class DemoController(object): z_incr = (z-curr_z)/segments for i in range(1, segments): - pose = self.cylinder2pose(curr_r + i*r_incr, curr_theta + i*theta_incr, curr_z + i*z_incr) - move_list.append(pose + [acc or self.acc, vel or self.vel, 0.03]) - move_list.append(self.cylinder2pose(r, theta, z) + [acc or self.acc, vel or self.vel, blend_last]) + 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=True): + + 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.movels(move_list, wait=wait) - self.current_cyl = (r, theta, z) - - - def blocklvl2pose(self, r_lvl, theta, z_lvl): - return self.cylinder2pose(self.r_lvl0 - r_lvl*self.block_dim, theta, self.z_lvl0 + z_lvl*self.block_dim) - - - def blocklvl2arc(self, r_lvl, theta, z_lvl, acc=None, vel=None, blend_last=0): - r = self.r_lvl0 - r_lvl*self.block_dim - z = self.z_lvl0 + z_lvl*self.block_dim - return self.linearize_arc(r, theta, z, acc=acc, vel=vel, blend_last=blend_last) - - - def pick_block(self, r_lvl, theta, z_lvl, speed): - """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. - """ - 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, speed, 0.005], - p2 + [self.acc, speed, 0], - p3 + [self.acc, speed, 0.01], - ] - return move_list - - def place_block(self, r_lvl, theta, z_lvl, speed): - """Reverse move of pick_block.""" - moves = self.pick_block(r_lvl, theta, z_lvl, speed) - moves.reverse() - return moves - - - def construct_moves(self, task_list, speed): - """Construct moves from a list of tasks""" - pass # wait with implementing this - - - 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 + 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: @@ -268,7 +215,6 @@ class DemoController(object): time.sleep(t) self.stopl(acc=self.chcmd_decel) - def update(self, vec, dt): """Update movements based on vec and dt""" @@ -320,5 +266,67 @@ class DemoController(object): 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() diff --git a/ur5controller.pyc b/ur5controller.pyc index 19f1382..c60bd22 100644 Binary files a/ur5controller.pyc and b/ur5controller.pyc differ