#!/usr/bin/env python # -*- coding: utf-8 -*- import sys, config, math, time, traceback from ur5controller import DemoController 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...') 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...') 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): 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_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(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 check_theta_edges(controller): controller.movec_hax(config.R_LVL0 - config.BLOCK_DIM, config.THETA_EDGE_RIGHT, config.Z_MIN) controller.movel(config.R_LVL0 - config.BLOCK_DIM, config.THETA_EDGE_RIGHT, config.Z_MIN) print 'Theta edge right.' dummy = raw_input('Press any key to continue...') controller.movec_hax(config.R_LVL0 - config.BLOCK_DIM, config.THETA_EDGE_LEFT, config.Z_MIN) controller.movel(config.R_LVL0 - config.BLOCK_DIM, config.THETA_EDGE_LEFT, config.Z_MIN) 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 #dtheta_lvl0 = config.BLOCK_DIM / config.R_LVL0 - math.pi/200#no gap # Initial: 1x2 left side #speed = 0.7 speed = 0.2 moves = [] moves += controller.pick_block(1, config.THETA_EDGE_LEFT, 0, speed) moves += controller.place_block(0, dtheta_lvl0/2, 0, speed) moves += controller.pick_block(0, config.THETA_EDGE_LEFT, 0, speed) moves += controller.place_block(0, -dtheta_lvl0/2, 0, speed) controller.movels(moves, wait=False) looping = True while looping: looping = controller.is_looping(1.0) # blocks for 1 sec moves.reverse() controller.movels(moves, wait=True) # Shit is smooth! def small_pyramid(controller): # Initial block placement. # On each side # (r0, edge, z0) # (r1, edge, z0) # (r0, edge, z1) dtheta_lvl0 = config.BLOCK_DIM / config.R_LVL0 + math.pi/64 #medium gap speed = 0.2 acc = config.ACCELERATION moves = [] # Pick left (r0, edge, z1), place in middle moves += controller.pick_block(0, config.THETA_EDGE_LEFT, 1, speed) tmp_pose = controller.blocklvl2pose(0.2, -dtheta_lvl0, 1.1) moves.append(tmp_pose + [acc, speed, 0.05]) moves += controller.place_block(0, 0, 0, speed) # Pick left (r1, edge, z0), place left moves += controller.pick_block(1, config.THETA_EDGE_LEFT, 0, speed) tmp_pose = controller.blocklvl2pose(1.1, config.THETA_EDGE_LEFT+dtheta_lvl0, 0.5) moves.append(tmp_pose + [acc, speed, 0.05]) moves += controller.place_block(0, -dtheta_lvl0, 0, speed) # Pick right (r0, edge, z1), place right tmp_pose = controller.blocklvl2pose(1.3, config.THETA_EDGE_RIGHT-dtheta_lvl0, 1.2) moves.append(tmp_pose + [acc, speed, 0.05]) #moves += controller.blocklvl2arc(1.3, config.THETA_EDGE_RIGHT - math.pi/16, 1, vel=speed, blend_last=0.03) moves += controller.pick_block(0, config.THETA_EDGE_RIGHT, 1, speed) tmp_pose = controller.blocklvl2pose(0, config.THETA_EDGE_RIGHT-dtheta_lvl0, 1.2) moves.append(tmp_pose + [acc, speed, 0.03]) moves += controller.place_block(0, dtheta_lvl0, 0, speed) # Pick right (r1, edge, z0), place lvl1 right moves += controller.pick_block(1, config.THETA_EDGE_RIGHT, 0, speed) tmp_pose = controller.blocklvl2pose(1.1, config.THETA_EDGE_RIGHT-dtheta_lvl0, 1.1) moves.append(tmp_pose + [acc, speed, 0.03]) moves += controller.place_block(0, 0.5*dtheta_lvl0, 1, speed) # Pick left (r0, edge, z0), place lvl1 left moves += controller.blocklvl2arc(1.3, config.THETA_EDGE_LEFT + math.pi/16, 0.1, vel=speed, blend_last=0.03) moves += controller.pick_block(0, config.THETA_EDGE_LEFT, 0, speed) tmp_pose = controller.blocklvl2pose(0, config.THETA_EDGE_LEFT+dtheta_lvl0, 1.1) moves.append(tmp_pose + [acc, speed, 0.03]) moves += controller.place_block(0, -0.5*dtheta_lvl0, 1, speed) # Pick right (r0, edge, z0), place on top moves += controller.blocklvl2arc(1.3, config.THETA_EDGE_RIGHT - math.pi/16, 0, vel=speed, blend_last=0.03) moves += controller.pick_block(0, config.THETA_EDGE_RIGHT, 0, speed) tmp_pose = controller.blocklvl2pose(0, config.THETA_EDGE_RIGHT-dtheta_lvl0, 2.1) moves.append(tmp_pose + [acc, speed, 0.03]) moves += controller.place_block(0, 0, 2, speed) # Execute moves controller.movels(moves, wait=False) looping = True while looping: looping = controller.is_looping(1.0) # blocks for 1 sec moves.reverse() controller.movels(moves, wait=True) looping = True while looping: looping = controller.is_looping(1.0) # blocks for 1 sec def big_pyramid(controller): dtheta_lvl0 = config.BLOCK_DIM / config.R_LVL0 + math.pi/64 #medium gap #dtheta_lvl0 = config.BLOCK_DIM / config.R_LVL0 #small gap #dtheta_lvl0 = config.BLOCK_DIM / config.R_LVL0 - math.pi/200#no gap # Initial block placement. # On each side # (r0, edge, z0) # (r1, edge, z0) # (r0, edge, z1) # (r1, edge, z1) # (r0, edge, z2) # (r1, edge, z2) speed = 0.2 moves = [] #move += controller.blocklvl2arc(0, config.THETA_EDGE_LEFT + math.pi/16, 1, vel=speed, blend_last=0.03) moves += controller.pick_block(0, config.THETA_EDGE_LEFT, 1, speed) # Pick left (r0, edge, z1) moves += controller.place_block(0, -0.5*dtheta_lvl0, 0) moves += controller.pick_block(1, config.THETA_EDGE_LEFT, 0, speed) # Pick left (r1, edge, z0) moves += controller.place_block(0, -1.5*dtheta_lvl0, 0) moves += controller.blocklvl2arc(0.1, config.THETA_EDGE_RIGHT - math.pi/16, 1, vel=speed, blend_last=0.03) moves += controller.pick_block(0, config.THETA_EDGE_RIGHT, 1, speed) # Pick right (r0, edge, z1) moves += controller.place_block(0, 0.5*dtheta_lvl0, 0) moves += controller.pick_block(1, config.THETA_EDGE_RIGHT, 0, speed) # Pick right (r1, edge, z0) 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_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(0)