#!/usr/bin/env python # -*- coding: utf-8 -*- 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.' dummy = raw_input('Press any key to continue...') controller.movel(config.R_MIN, 0, config.Z_MIN) print 'Middle.' time.sleep(1) 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) 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.' time.sleep(1) 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) 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.' 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) 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) 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. except Exception, e: print e print traceback.format_exc() time.sleep(1) if controller: controller.cleanup() sys.exit(1)