356 lines
12 KiB
Python
Executable File
356 lines
12 KiB
Python
Executable File
#!/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)
|