From 5ee0353e0109a929a6272415571e450c1fa529be Mon Sep 17 00:00:00 2001 From: Michael Soukup Date: Tue, 5 Aug 2014 20:45:14 +0200 Subject: [PATCH] Two loops complete and tested: build 4 stories high and small pyramid --- unittesting.py | 188 +++++++++++++++++++++------------------------- ur5controller.py | 5 +- ur5controller.pyc | Bin 12524 -> 12502 bytes 3 files changed, 86 insertions(+), 107 deletions(-) diff --git a/unittesting.py b/unittesting.py index c834806..b58e109 100755 --- a/unittesting.py +++ b/unittesting.py @@ -178,139 +178,117 @@ def test_movec(controller): +def four_stories(controller): + print "Initial block placement." + print "On each side" + print "\t(r0, edge, z0)" + print "\t(r1, edge, z0)" -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 + # buffer some params + edge_left = config.THETA_EDGE_LEFT + edge_right = config.THETA_EDGE_RIGHT + r_startpick = 1.3 + r_endpick = 0.1 + z_endpick = 0.5 + vel = 0.2 + acc = 1.1*vel 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) - + + moves.append(controller.blocklevel2move(0.5, 0, 0, acc, vel, 0)) + moves += controller.pick_block(1, edge_left, 0, acc, vel) + moves += controller.place_block(0, 0, 0, acc, vel) + + moves += controller.pick_block(1, edge_right, 0, acc, vel) + moves.append(controller.blocklevel2move(1.1, edge_right/2, 2, acc, vel, 0.15)) + moves += controller.place_block(0, 0, 1, acc, vel) + + moves += controller.pick_block(0, edge_left, 0, acc, vel) + moves.append(controller.blocklevel2move(0.1, edge_left/2, 3, acc, vel, 0.15)) + moves += controller.place_block(0, 0, 2, acc, vel) + + moves += controller.pick_block(0, edge_right, 0, acc, vel) + moves.append(controller.blocklevel2move(0.1, edge_right/2, 4, acc, vel, 0.15)) + moves += controller.place_block(0, 0, 3, acc, vel) + + moves.append(controller.blocklevel2move(1.5, 0, 0.1, acc, vel, 0)) + + dummy = raw_input("Press enter to start...") + print 'Exectuting loop' + controller.exec_moves(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! + controller.exec_moves(moves, wait=False) + looping = True + while looping: + looping = controller.is_looping(1.0) # blocks for 1 sec def small_pyramid(controller): - # Initial block placement. - # On each side - # (r0, edge, z0) - # (r1, edge, z0) - # (r0, edge, z1) + print "Initial block placement." + print "On each side" + print "\t(r0, edge, z0)" + print "\t(r1, edge, z0)" + print "\t(r0, edge, z1)" - dtheta_lvl0 = config.BLOCK_DIM / config.R_LVL0 + math.pi/64 #medium gap - speed = 0.2 - acc = config.ACCELERATION + # buffer some params + edge_left = config.THETA_EDGE_LEFT + edge_right = config.THETA_EDGE_RIGHT + r_startpick = 1.3 + r_endpick = 0.1 + z_endpick = 0.5 + + vel = 0.2 + acc = 1.1*vel + dtheta_lvl0 = config.BLOCK_DIM/config.R_LVL0 + math.pi/64 #medium gap 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) + moves.append(controller.blocklevel2move(0.5, 0, 0, acc, vel, 0)) #initial + moves += controller.pick_block(0, edge_left, 1, acc, vel) + moves.append(controller.blocklevel2move(0.1, edge_left/2, 2, acc, vel, 0.15)) + moves += controller.place_block(0, 0, 0, acc, vel) - # 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) + moves += controller.pick_block(0, edge_right, 1, acc, vel) + moves.append(controller.blocklevel2move(0.1, (edge_right+dtheta_lvl0)/2, 2, acc, vel, 0.15)) + moves += controller.place_block(0, dtheta_lvl0, 0, acc, vel) - # 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) + moves.append(controller.blocklevel2move(1.1, (edge_left+dtheta_lvl0)/2, 0.5, acc, vel, 0.15)) + moves += controller.pick_block(1, edge_left, 0, acc, vel) + moves += controller.place_block(0, -dtheta_lvl0, 0, acc, vel) - # 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) + moves.append(controller.blocklevel2move(1.1, (edge_right-dtheta_lvl0)/2, 0.5, acc, vel, 0.15)) + moves += controller.pick_block(1, edge_right, 0, acc, vel) + moves.append(controller.blocklevel2move(1.1, edge_right/2, 2, acc, vel, 0.15)) + moves += controller.place_block(0, 0.5*dtheta_lvl0, 1, acc, vel) - # 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) + moves += controller.pick_block(0, edge_left, 0, acc, vel) + moves.append(controller.blocklevel2move(0.1, edge_left/2, 2, acc, vel, 0.15)) + moves += controller.place_block(0, -0.5*dtheta_lvl0, 1, acc, vel) - # 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) + moves.append(controller.blocklevel2move(1.1, (edge_right-dtheta_lvl0)/2, 0.5, acc, vel, 0.15)) + moves += controller.pick_block(0, edge_right, 0, acc, vel) + moves.append(controller.blocklevel2move(0.1, edge_right/2, 3, acc, vel, 0.15)) + moves += controller.place_block(0, 0, 2, acc, vel) - # Execute moves - controller.movels(moves, wait=False) + moves.append(controller.blocklevel2move(1.5, 0, 0.1, acc, vel, 0)) + + dummy = raw_input("Press enter to start...") + print 'Exectuting loop' + controller.exec_moves(moves, wait=False) looping = True while looping: looping = controller.is_looping(1.0) # blocks for 1 sec moves.reverse() - controller.movels(moves, wait=True) + controller.exec_moves(moves, wait=False) 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 @@ -345,7 +323,9 @@ if __name__ == '__main__': ("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) + ("Test URScript's movec", test_movec), + ("Build four stories high", four_stories), + ("Build small pyramid", small_pyramid) ] unit_tests(tests, controller) diff --git a/ur5controller.py b/ur5controller.py index 35b3932..916e222 100644 --- a/ur5controller.py +++ b/ur5controller.py @@ -272,11 +272,10 @@ class DemoController(object): 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 + def blocklevel2move(self, r_lvl, theta, z_lvl, acc, vel, blend): 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] + move = self.cylinder2cartesian(r, theta, z) + [acc, vel, blend] return move diff --git a/ur5controller.pyc b/ur5controller.pyc index b4f34fb5c08599b3bf576a9f2607c39e2d584bd4..656e61d3df1ec271ad843d97b4343ba53d84259a 100644 GIT binary patch delta 222 zcmaEpcrB5Q`7G1M@F8H@}f z;m=CwprsF>+1L)6ik;pFB+?m637t wAB|LIMybt7I*g2rCX-Wik26|N*3nzc=rH+$-b6;{$w~S@nK;=e#~Snl0AOk{+5i9m delta 219 zcmcbX_$HB!`7U8O!m?L$t1)v+1;QY0AX1) ATmS$7