michael/ur5demo
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity

Two loops complete and tested: build 4 stories high and small pyramid

Browse Source
This commit is contained in:
Michael Soukup 2014-08-05 20:45:14 +02:00
parent fc7149240e
commit 5ee0353e01
3 changed files with 86 additions and 107 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

184
unittesting.py
View File

@ -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): # buffer some params
controller.movec_hax(config.R_LVL0 - config.BLOCK_DIM, config.THETA_EDGE_RIGHT, config.Z_MIN) edge_left = config.THETA_EDGE_LEFT
controller.movel(config.R_LVL0 - config.BLOCK_DIM, config.THETA_EDGE_RIGHT, config.Z_MIN) edge_right = config.THETA_EDGE_RIGHT
print 'Theta edge right.' r_startpick = 1.3
dummy = raw_input('Press any key to continue...') r_endpick = 0.1
z_endpick = 0.5
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
vel = 0.2
acc = 1.1*vel
moves = [] 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 looping = True
while looping: while looping:
looping = controller.is_looping(1.0) # blocks for 1 sec looping = controller.is_looping(1.0) # blocks for 1 sec
moves.reverse() moves.reverse()
controller.movels(moves, wait=True) controller.exec_moves(moves, wait=False)
# Shit is smooth! looping = True
while looping:
looping = controller.is_looping(1.0) # blocks for 1 sec
def small_pyramid(controller): def small_pyramid(controller):
# Initial block placement. print "Initial block placement."
# On each side print "On each side"
# (r0, edge, z0) print "\t(r0, edge, z0)"
# (r1, edge, z0) print "\t(r1, edge, z0)"
# (r0, edge, z1) print "\t(r0, edge, z1)"
# 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 dtheta_lvl0 = config.BLOCK_DIM/config.R_LVL0 + math.pi/64 #medium gap
speed = 0.2
acc = config.ACCELERATION
moves = [] moves = []
# Pick left (r0, edge, z1), place in middle moves.append(controller.blocklevel2move(0.5, 0, 0, acc, vel, 0)) #initial
moves += controller.pick_block(0, config.THETA_EDGE_LEFT, 1, speed) moves += controller.pick_block(0, edge_left, 1, acc, vel)
tmp_pose = controller.blocklvl2pose(0.2, -dtheta_lvl0, 1.1) moves.append(controller.blocklevel2move(0.1, edge_left/2, 2, acc, vel, 0.15))
moves.append(tmp_pose + [acc, speed, 0.05]) moves += controller.place_block(0, 0, 0, acc, vel)
moves += controller.place_block(0, 0, 0, speed)
# Pick left (r1, edge, z0), place left moves += controller.pick_block(0, edge_right, 1, acc, vel)
moves += controller.pick_block(1, config.THETA_EDGE_LEFT, 0, speed) moves.append(controller.blocklevel2move(0.1, (edge_right+dtheta_lvl0)/2, 2, acc, vel, 0.15))
tmp_pose = controller.blocklvl2pose(1.1, config.THETA_EDGE_LEFT+dtheta_lvl0, 0.5) moves += controller.place_block(0, dtheta_lvl0, 0, acc, vel)
moves.append(tmp_pose + [acc, speed, 0.05])
moves += controller.place_block(0, -dtheta_lvl0, 0, speed)
# Pick right (r0, edge, z1), place right moves.append(controller.blocklevel2move(1.1, (edge_left+dtheta_lvl0)/2, 0.5, acc, vel, 0.15))
tmp_pose = controller.blocklvl2pose(1.3, config.THETA_EDGE_RIGHT-dtheta_lvl0, 1.2) moves += controller.pick_block(1, edge_left, 0, acc, vel)
moves.append(tmp_pose + [acc, speed, 0.05]) moves += controller.place_block(0, -dtheta_lvl0, 0, acc, vel)
#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.append(controller.blocklevel2move(1.1, (edge_right-dtheta_lvl0)/2, 0.5, acc, vel, 0.15))
moves += controller.pick_block(1, config.THETA_EDGE_RIGHT, 0, speed) moves += controller.pick_block(1, edge_right, 0, acc, vel)
tmp_pose = controller.blocklvl2pose(1.1, config.THETA_EDGE_RIGHT-dtheta_lvl0, 1.1) moves.append(controller.blocklevel2move(1.1, edge_right/2, 2, acc, vel, 0.15))
moves.append(tmp_pose + [acc, speed, 0.03]) moves += controller.place_block(0, 0.5*dtheta_lvl0, 1, acc, vel)
moves += controller.place_block(0, 0.5*dtheta_lvl0, 1, speed)
# Pick left (r0, edge, z0), place lvl1 left moves += controller.pick_block(0, edge_left, 0, acc, vel)
moves += controller.blocklvl2arc(1.3, config.THETA_EDGE_LEFT + math.pi/16, 0.1, vel=speed, blend_last=0.03) moves.append(controller.blocklevel2move(0.1, edge_left/2, 2, acc, vel, 0.15))
moves += controller.pick_block(0, config.THETA_EDGE_LEFT, 0, speed) moves += controller.place_block(0, -0.5*dtheta_lvl0, 1, acc, vel)
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.append(controller.blocklevel2move(1.1, (edge_right-dtheta_lvl0)/2, 0.5, acc, vel, 0.15))
moves += controller.blocklvl2arc(1.3, config.THETA_EDGE_RIGHT - math.pi/16, 0, vel=speed, blend_last=0.03) moves += controller.pick_block(0, edge_right, 0, acc, vel)
moves += controller.pick_block(0, config.THETA_EDGE_RIGHT, 0, speed) moves.append(controller.blocklevel2move(0.1, edge_right/2, 3, acc, vel, 0.15))
tmp_pose = controller.blocklvl2pose(0, config.THETA_EDGE_RIGHT-dtheta_lvl0, 2.1) moves += controller.place_block(0, 0, 2, acc, vel)
moves.append(tmp_pose + [acc, speed, 0.03])
moves += controller.place_block(0, 0, 2, speed)
# Execute moves moves.append(controller.blocklevel2move(1.5, 0, 0.1, acc, vel, 0))
controller.movels(moves, wait=False)
dummy = raw_input("Press enter to start...")
print 'Exectuting loop'
controller.exec_moves(moves, wait=False)
looping = True looping = True
while looping: while looping:
looping = controller.is_looping(1.0) # blocks for 1 sec looping = controller.is_looping(1.0) # blocks for 1 sec
moves.reverse() moves.reverse()
controller.movels(moves, wait=True) controller.exec_moves(moves, wait=False)
looping = True looping = True
while looping: while looping:
looping = controller.is_looping(1.0) # blocks for 1 sec 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): def unit_tests(func_list, controller):
running = True running = True
@ -345,7 +323,9 @@ if __name__ == '__main__':
("Test unitary moves.", test_unitary_moves), ("Test unitary moves.", test_unitary_moves),
("Test multi-directional moves", test_multi_moves), ("Test multi-directional moves", test_multi_moves),
("Check loop constraints", check_loop_constraints), ("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) unit_tests(tests, controller)

5
ur5controller.py
View File

@ -272,11 +272,10 @@ class DemoController(object):
return sum(map(abs, v)) > threshold return sum(map(abs, v)) > threshold
def blocklevel2move(self, r_lvl, theta, z_lvl, acc=0.5, vel=0.1, r=0): def blocklevel2move(self, r_lvl, theta, z_lvl, acc, vel, blend):
# loop acc/vel
r = self.r_lvl0 - r_lvl*self.block_dim r = self.r_lvl0 - r_lvl*self.block_dim
z = self.z_lvl0 + z_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 return move

BIN
ur5controller.pyc
View File

Binary file not shown.