diff --git a/config.py b/config.py index bf3f9af..62559e5 100644 --- a/config.py +++ b/config.py @@ -27,8 +27,7 @@ UR5_HOSTNAME = 'ur-2012208984' #requires dns. # BLOCK DIMENSION # # Blocks are cubic and the hole is centered. -BLOCK_DIM = 0.0975 #not exactly 10cm - +BLOCK_DIM = 0.0965 #TUNED # TABLE CONFIGURATION @@ -42,8 +41,8 @@ BLOCK_DIM = 0.0975 #not exactly 10cm # 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_ORIGO_OFFSET = (-0.245, -0.003) #offset from origo along base x-axis +TABLE_Z = -0.3337 #this should be fine-tuned on each setup TABLE_ARC = math.pi/2 @@ -57,9 +56,9 @@ TABLE_ARC = math.pi/2 # coordinate system. # # Change joints 2-6 in QUADRANT_JOINTS_HOME to tune the tool orientation. -R_HOME = 0.5 +R_HOME = 0.55 THETA_HOME = 0 -Z_HOME = TABLE_Z + 2*BLOCK_DIM +Z_HOME = TABLE_Z + BLOCK_DIM JOINTS_HOME = [TABLE_QUADRANT*math.pi/2, -3*math.pi/4, -3*math.pi/4, @@ -67,8 +66,6 @@ JOINTS_HOME = [TABLE_QUADRANT*math.pi/2, -math.pi/2, 49*math.pi/50] -<<<<<<< HEAD - # FORCE MONITOR # @@ -120,49 +117,10 @@ LOOP_VEL_MIN = 0.05 LOOP_VEL_MAX = 0.7 LOOP_ACC = 0.5 -R_LVL0 = 0.7 #tip of tool at table edge +R_LVL0 = 0.691 #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 -======= -# 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. -USE_FORCE_MONITOR = True -FORCE_CONSTRAINT = 60 -FORCE_T = 0.1 - -# Control parameters. -VELOCITY = 0.1 -ACCELERATION = 0.5 -DECELERATION = 0.5 -CHCMD_DECEL = 0.8 -CHCMD_T = 0.01 # 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 - -# 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.6 # relative to TABLE center -R_MAX = 0.9 # 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 - -# 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 ->>>>>>> parent of ccb5ffb... manual controller done. Going to separate control parameters for manual mode and auto mode -THETA_EDGE_RIGHT = math.pi/4 - math.pi/68 +THETA_EDGE_LEFT = -math.pi/4 + math.pi/74 #empirical. table is not exact +THETA_EDGE_RIGHT = math.pi/4 - math.pi/74 # Offsets when picking (and placing) blocks. # The place-block move is the reverse of the pick-block move, diff --git a/config.pyc b/config.pyc index ba0cef4..b195b2a 100644 Binary files a/config.pyc and b/config.pyc differ diff --git a/unittesting.py b/unittesting.py index 04a0fb3..c834806 100755 --- a/unittesting.py +++ b/unittesting.py @@ -5,66 +5,179 @@ import sys, config, math, time, traceback from ur5controller import DemoController -def check_edges(controller): - controller.movel(config.R_MIN, config.THETA_MAX, config.Z_MIN) - print 'Right edge.' +def check_z_levels(controller): + print 'Going to home pose.' + controller.move_to_home_pose() + time.sleep(1) + + r, theta, z = controller.current_cyl + + print "z-level 0" + z = config.Z_LVL0 + move = controller.cylinder2cartesian(r, theta, z) + [0.2, 0.1, 0] + controller.exec_move(move, wait=True) + controller.set_cylinder_coords() dummy = raw_input('Press any key to continue...') - controller.movel(config.R_MIN, 0, config.Z_MIN) - print 'Middle.' - time.sleep(1) + print "z-level 1" + z += config.BLOCK_DIM + move = controller.cylinder2cartesian(r, theta, z) + [0.2, 0.1, 0] + controller.exec_move(move, wait=True) + controller.set_cylinder_coords() dummy = raw_input('Press any key to continue...') - controller.movel(config.R_MIN, config.THETA_MIN, config.Z_MIN) - print 'Left edge.' - time.sleep(1) + print "z-level 2" + z += config.BLOCK_DIM + move = controller.cylinder2cartesian(r, theta, z) + [0.2, 0.1, 0] + controller.exec_move(move, wait=True) + controller.set_cylinder_coords() dummy = raw_input('Press any key to continue...') + print "z-level 3" + z += config.BLOCK_DIM + move = controller.cylinder2cartesian(r, theta, z) + [0.2, 0.1, 0] + controller.exec_move(move, wait=True) + controller.set_cylinder_coords() + dummy = raw_input('Press any key to continue...') + + print "z-level 4" + z += config.BLOCK_DIM + move = controller.cylinder2cartesian(r, theta, z) + [0.2, 0.1, 0] + controller.exec_move(move, wait=True) + controller.set_cylinder_coords() + dummy = raw_input('Press any key to continue...') + + +def test_unitary_moves(controller): + print 'Going to home pose.' + controller.move_to_home_pose() + time.sleep(1) + + print 'Moving (+r, 0, 0)' + controller.move((1,0,0), t=2) + time.sleep(1) + controller.set_cylinder_coords() + + print 'Moving (-r, 0, 0)' + controller.move((-1,0,0), t=2) + time.sleep(1) + controller.set_cylinder_coords() + + print 'Moving (0, +theta, 0)' + controller.move((0,1,0), t=2) + time.sleep(1) + controller.set_cylinder_coords() + + print 'Moving (0, -theta, 0)' + controller.move((0,-1,0), t=2) + time.sleep(1) + controller.set_cylinder_coords() + + print 'Moving (0, 0, +z)' + controller.move((0,0,1), t=2) + time.sleep(1) + controller.set_cylinder_coords() + + print 'Moving (0, 0, -z)' + controller.move((0,0,-1), t=2) + time.sleep(1) + controller.set_cylinder_coords() + + +def test_multi_moves(controller): + print 'Going to home pose.' + controller.move_to_home_pose() + time.sleep(1) + + print 'Moving (+r, +theta, 0)' + controller.move((1,1,0), t=2) + time.sleep(1) + controller.set_cylinder_coords() + + print 'Moving (-r, -theta, 0)' + controller.move((-1,-1,0), t=2) + time.sleep(1) + controller.set_cylinder_coords() + + print 'Moving (+r, -theta, 0)' + controller.move((1,-1,0), t=2) + time.sleep(1) + controller.set_cylinder_coords() + + print 'Moving (+r, 0, z)' + controller.move((1,0,1), t=2) + time.sleep(1) + controller.set_cylinder_coords() + + print 'Moving (-r, 0, z)' + controller.move((-1,0,1), t=2) + time.sleep(1) + controller.set_cylinder_coords() + + print 'Moving (0, theta, -z)' + controller.move((0,1,-1), t=2) + time.sleep(1) + controller.set_cylinder_coords() + + print 'Moving (0, -theta, -z)' + controller.move((0,-1,-1), t=2) + time.sleep(1) + controller.set_cylinder_coords() + + print 'Moving (r, theta, z)' + controller.move((1,1,1), t=2) + time.sleep(1) + controller.set_cylinder_coords() + + print 'Moving (-r, theta, -z)' + controller.move((-1,1,-1), t=2) + time.sleep(1) + controller.set_cylinder_coords() + + +def check_loop_constraints(controller): + print 'Going to home pose.' + controller.move_to_home_pose() + time.sleep(1) + + controller.movec_hax(config.R_LVL0, config.THETA_EDGE_RIGHT, config.Z_LVL0, wait=True) + controller.set_cylinder_coords() + print 'Right edge, r-level 0, z-level 0. Cylinder coordinates: (%s)' % ','.join(['%.2f' % i for i in controller.current_cyl]) + dummy = raw_input('Press any key to continue...') + + controller.movec_hax(config.R_LVL0, 0, config.Z_LVL0, wait=True) + controller.set_cylinder_coords() + print 'Middle, r-level 0, z-level 0. Cylinder coordinates: (%s)' % ','.join(['%.2f' % i for i in controller.current_cyl]) + dummy = raw_input('Press any key to continue...') + + controller.movec_hax(config.R_LVL0, config.THETA_EDGE_LEFT, config.Z_LVL0, wait=True) + controller.set_cylinder_coords() + print 'Left edge, r-level 0, z-level 0. Cylinder coordinates: (%s)' % ','.join(['%.2f' % i for i in controller.current_cyl]) + dummy = raw_input('Press any key to continue...') + + def test_movec(controller): - controller.movec(config.R_MIN, config.THETA_MAX, config.Z_MIN) - print 'Right edge.' - dummy = raw_input('Press any key to continue...') - - controller.movec(config.R_MIN, 0, config.Z_MIN) - print 'Middle.' + print 'Going to home pose.' + controller.move_to_home_pose() time.sleep(1) + + controller.movec(config.R_LVL0, config.THETA_EDGE_RIGHT, config.Z_LVL0, wait=True) + controller.set_cylinder_coords() + print 'Right edge, r-level 0, z-level 0. Cylinder coordinates: (%s)' % ','.join(['%.2f' % i for i in controller.current_cyl]) dummy = raw_input('Press any key to continue...') - controller.movec(config.R_MIN, config.THETA_MIN, config.Z_MIN) - print 'Left edge.' - time.sleep(1) + controller.movec(config.R_LVL0, 0, config.Z_LVL0, wait=True) + controller.set_cylinder_coords() + print 'Middle, r-level 0, z-level 0. Cylinder coordinates: (%s)' % ','.join(['%.2f' % i for i in controller.current_cyl]) dummy = raw_input('Press any key to continue...') -def test_movec_hax(controller): - controller.movec_hax(config.R_MIN, config.THETA_MAX, config.Z_MIN) - print 'Right edge.' + controller.movec(config.R_LVL0, config.THETA_EDGE_LEFT, config.Z_LVL0, wait=True) + controller.set_cylinder_coords() + print 'Left edge, r-level 0, z-level 0. Cylinder coordinates: (%s)' % ','.join(['%.2f' % i for i in controller.current_cyl]) dummy = raw_input('Press any key to continue...') - controller.movec_hax(config.R_MIN, 0, config.Z_MIN) - print 'Middle.' - time.sleep(1) - dummy = raw_input('Press any key to continue...') - controller.movec_hax(config.R_MIN, config.THETA_MIN, config.Z_MIN) - print 'Left edge.' - time.sleep(1) - dummy = raw_input('Press any key to continue...') -def test_movec_hax2(controller): - controller.movec_hax(config.R_MIN, config.THETA_MAX, config.Z_MIN) - print 'Right edge.' - dummy = raw_input('Press any key to continue...') - - controller.movec_hax(config.R_MIN, 0, config.Z_MIN+0.3) - print 'Middle.' - time.sleep(1) - dummy = raw_input('Press any key to continue...') - - # NOTE: current R_MIN+0.1=0.7 is where the tooltip is at the edge - controller.movec_hax(config.R_MIN+0.1, config.THETA_MIN, config.Z_MIN+0.1) - print 'Left edge.' - time.sleep(1) - dummy = raw_input('Press any key to continue...') def check_theta_edges(controller): controller.movec_hax(config.R_LVL0 - config.BLOCK_DIM, config.THETA_EDGE_RIGHT, config.Z_MIN) @@ -77,6 +190,7 @@ def check_theta_edges(controller): print 'Theta edge left.' dummy = raw_input('Press any key to continue...') + def test_pick_place(controller): dtheta_lvl0 = config.BLOCK_DIM / config.R_LVL0 + math.pi/64 #medium gap # #dtheta_lvl0 = config.BLOCK_DIM / config.R_LVL0 #small gap @@ -198,26 +312,44 @@ def big_pyramid(controller): moves += controller.place_block(0, 1.5*dtheta_lvl0, 0) +def unit_tests(func_list, controller): + running = True + while running: + for i,tup in enumerate(func_list): + print '(%d) %s' % (i, tup[0]) + c = raw_input('Select test or type q to quit... ') + if c == 'q': + running = False + else: + try: + idx = int(c) + f = func_list[idx][1] + f(controller) + except Exception, e: + print e + print traceback.format_exc() + running = False + if __name__ == '__main__': controller = None try: controller = DemoController(config) - controller.calibrate_csys() - #controller.movel(config.R_LVL0, 0, config.Z_MIN + config.BLOCK_DIM) - #check_edges(controller) - #test_movec_hax(controller) - #check_theta_edges(controller) - #test_pick_place(controller) - #pick_place_small_pyramid(controller) - small_pyramid(controller) - - # NEXT: test radial params and arbitrary paths. + controller.calibrate_cylinder_sys() except Exception, e: print e print traceback.format_exc() + tests = [ + ("Check z-levels", check_z_levels), + ("Test unitary moves.", test_unitary_moves), + ("Test multi-directional moves", test_multi_moves), + ("Check loop constraints", check_loop_constraints), + ("Test URScript's movec", test_movec) + ] + unit_tests(tests, controller) + time.sleep(1) if controller: controller.cleanup() - sys.exit(1) + sys.exit(0) diff --git a/ur5controller.py b/ur5controller.py index b2c9d3b..35b3932 100644 --- a/ur5controller.py +++ b/ur5controller.py @@ -24,8 +24,8 @@ class DemoController(object): 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) + self.cyl_ox = self.cyl_offset[0]*math.cos(self.phi) + self.cyl_oy = self.cyl_offset[1]*math.sin(self.phi) # Home pose params self.j_home = config.JOINTS_HOME @@ -72,12 +72,12 @@ class DemoController(object): 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) + self.robot.movej(self.j_home, acc=0.5, vel=0.4, 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.robot.movel(pose, acc=0.15, 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]) @@ -94,8 +94,7 @@ class DemoController(object): 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 + self.cyl_ox, self.cyl_oy = self.cyl_offset def set_cylinder_coords(self): @@ -175,7 +174,7 @@ class DemoController(object): return move_list - def movec_hax(self, r, theta, z, blend_last=0, wait=False): + def movec_hax(self, r, theta, z, wait=False): """movec with linearized arc.""" # manual acc/vel move_list = self.linearize_arc(r, theta, z) @@ -188,43 +187,43 @@ class DemoController(object): def move(self, vec, t=0): """Move in vec direction. Assumes that |v| != 0""" - vr, vtheta, vz = vec + dr, dtheta, dz = vec r, theta, z = self.current_cyl - if vtheta == 0: - # linear move - if vz == 0: - move = + if dtheta == 0: #linear move + if dz == 0: + move = self.cylinder2cartesian(r+dr, theta, z) + [self.acc, self.vel, 0] + else: + if dr == 0: + move = self.cylinder2cartesian(r, theta, z+dz) + [self.acc, self.vel_z, 0] + else: + move = self.cylinder2cartesian(r+dr, theta, z+dz*(self.vel_z/self.vel)) + [self.acc, self.vel, 0] + self.exec_move(move) - else: - # arc move + else: #arc move + dtheta = dtheta/r # angle of arc segment with length 1 + if dz == 0: + moves = self.linearize_arc(r+dr, theta+dtheta, z) + else: + moves = self.linearize_arc(r+dr, theta+dtheta, z+dz*(self.vel_z/self.vel)) + self.exec_moves(moves) - - 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: + if t != 0: time.sleep(t) - self.stopl(acc=self.chcmd_decel) + self.robot.stopl(acc=self.stop_acc) + 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.move([-1*i for i in vec], t=self.t_force) self.set_current_cyl() return self.set_current_cyl() - vr, vtheta, vz = vec + dr, dtheta, dz = vec r, theta, z = self.current_cyl # move? @@ -234,30 +233,27 @@ class DemoController(object): prev_vec = (0,0,0) return - # check bounds - rnext = r + (vr*self.vel*dt) + # check projected constraints + rnext = r + (dr*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 + dr = 0 + thetanext = theta + (dtheta*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) + dtheta = 0 + znext = z + (dz*self.vel_z*dt) if znext < self.z_min or znext > self.z_max: - vz = 0 - vec = (vr, vtheta, vz) + dz = 0 + vec = (dr, dtheta, dz) - # command change + # change move if vec != self.prev_vec: - # from stand still - if sum(map(abs, self.prev_vec)) == 0: + if sum(map(abs, self.prev_vec)) == 0: #from still self.move(vec) self.prev_vec = vec self.t_ch = 0 - - # from another command - elif not self.t_ch: + elif not self.t_ch: #from another command self.t_ch = time.time() - self.robot.stopl(acc=self.chcmd_decel) + self.robot.stopl(acc=self.stop_acc) elif time.time() - self.t_ch > self.t_chcmd: self.move(vec) self.prev_vec = vec @@ -278,8 +274,10 @@ class DemoController(object): 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) - + r = self.r_lvl0 - r_lvl*self.block_dim + z = self.z_lvl0 + z_lvl*self.block_dim + move = self.cylinder2cartesian(r, theta, z) + [acc, vel, r] + return move def pick_block(self, r_lvl, theta, z_lvl, acc, vel): @@ -295,12 +293,11 @@ class DemoController(object): 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) + r = self.r_lvl0 - r_lvl*self.block_dim + z = self.z_lvl0 + z_lvl*self.block_dim + p1 = self.cylinder2cartesian(r+self.r_pick_init, theta+self.theta_pick_init, z+self.z_pick_init) + p2 = self.cylinder2cartesian(r, theta, z) + p3 = self.cylinder2cartesian(r+self.r_pick_end, theta+self.theta_pick_end, z+self.z_pick_end) move_list = [ p1 + [self.acc, vel, 0.005], p2 + [self.acc, vel, 0], @@ -311,8 +308,7 @@ class DemoController(object): 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 = self.pick_block(r_lvl, theta, z_lvl, acc, vel) moves.reverse() return moves diff --git a/ur5controller.pyc b/ur5controller.pyc index c60bd22..b4f34fb 100644 Binary files a/ur5controller.pyc and b/ur5controller.pyc differ